Francis CHATAIN / Mbed 2 deprecated MSNV2-Terminal_V1-6

Version FC

Dependencies:   DmTftLibrary eeprom SX1280Lib filesystem mbed

Fork of MSNV2-Terminal_V1-5 by Francis CHATAIN

main.cpp@4:41a92fbf702a, 2017-08-13 (annotated)

Committer:
FCH_31
Date:
Sun Aug 13 19:45:38 2017 +0000
Revision:
4:41a92fbf702a
Parent:
3:3adb454ba1d2
Child:
5:aa8a8d1a5aa7
Version ASCII OK;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GregCr 1:22e02d1cfbca 1 #include "mbed.h"
GregCr 1:22e02d1cfbca 2 #include "radio.h"
GregCr 1:22e02d1cfbca 3 #include "sx1280-hal.h"
FCH_31 3:3adb454ba1d2 4 #include "stdio.h"
FCH_31 4:41a92fbf702a 5 #include "XNucleoIKS01A2.h"
GregCr 1:22e02d1cfbca 6
FCH_31 4:41a92fbf702a 7 #define FIRMWARE_VERSION ( ( char* )"Firmware Version: 0.1" ) // display firmware version on RS232
FCH_31 2:57f098de07c7 8 #define MODE_LORA // Lora modulation
FCH_31 2:57f098de07c7 9 #define RF_FREQUENCY 2400000000UL // HzNominal frequency
FCH_31 4:41a92fbf702a 10 #define TX_OUTPUT_POWER -18 // Output power in dBm [-18..+13] dBm
GregCr 1:22e02d1cfbca 11
FCH_31 2:57f098de07c7 12 typedef enum // States of the application
GregCr 1:22e02d1cfbca 13 {
GregCr 1:22e02d1cfbca 14 APP_LOWPOWER,
GregCr 1:22e02d1cfbca 15 APP_RX,
GregCr 1:22e02d1cfbca 16 APP_RX_TIMEOUT,
GregCr 1:22e02d1cfbca 17 APP_RX_ERROR,
GregCr 1:22e02d1cfbca 18 APP_TX,
GregCr 1:22e02d1cfbca 19 APP_TX_TIMEOUT,
GregCr 1:22e02d1cfbca 20 }AppStates_t;
GregCr 1:22e02d1cfbca 21
FCH_31 3:3adb454ba1d2 22 #define ID_TERMINAL "1"
FCH_31 3:3adb454ba1d2 23 #define ID_GATEWAY "1"
FCH_31 3:3adb454ba1d2 24
FCH_31 3:3adb454ba1d2 25 float temperature = 0.0 ;
FCH_31 3:3adb454ba1d2 26 float humidite = 0.0 ;
FCH_31 3:3adb454ba1d2 27
GregCr 1:22e02d1cfbca 28
FCH_31 3:3adb454ba1d2 29 #define MESSAGESIZE 34 // Defines the size of the token defining message type in the payload
FCH_31 3:3adb454ba1d2 30 char Message[MESSAGESIZE] = "MSG DATA IOT TERMINAL";
GregCr 1:22e02d1cfbca 31
FCH_31 3:3adb454ba1d2 32 int cptr = 0 ;
GregCr 1:22e02d1cfbca 33
FCH_31 3:3adb454ba1d2 34 #define BUFFER_SIZE 34 // Payload size
FCH_31 3:3adb454ba1d2 35 uint8_t BufferSize = BUFFER_SIZE; // Size of the buffer
FCH_31 2:57f098de07c7 36 uint8_t Buffer[BUFFER_SIZE]; // Buffer
GregCr 1:22e02d1cfbca 37
FCH_31 2:57f098de07c7 38 AppStates_t AppState = APP_LOWPOWER; // State of the application
GregCr 1:22e02d1cfbca 39
FCH_31 3:3adb454ba1d2 40 int8_t RssiValue = 0;
FCH_31 3:3adb454ba1d2 41 int8_t SnrValue = 0;
GregCr 1:22e02d1cfbca 42
FCH_31 2:57f098de07c7 43 void OnTxDone ( void ); // Function to be executed on Radio Tx Done event
FCH_31 2:57f098de07c7 44 void OnRxDone ( void ); // Function to be executed on Radio Rx Done event
FCH_31 2:57f098de07c7 45 void OnTxTimeout ( void ); // Function executed on Radio Tx Timeout event
FCH_31 2:57f098de07c7 46 void OnRxTimeout ( void ); // Function executed on Radio Rx Timeout event
FCH_31 2:57f098de07c7 47 void OnRxError ( IrqErrorCode_t ); // Function executed on Radio Rx Error event
GregCr 1:22e02d1cfbca 48
FCH_31 2:57f098de07c7 49 RadioCallbacks_t callbacks = // All the callbacks are stored in a structure
GregCr 1:22e02d1cfbca 50 {
GregCr 1:22e02d1cfbca 51 &OnTxDone, // txDone
GregCr 1:22e02d1cfbca 52 &OnRxDone, // rxDone
GregCr 1:22e02d1cfbca 53 NULL, // syncWordDone
GregCr 1:22e02d1cfbca 54 NULL, // headerDone
GregCr 1:22e02d1cfbca 55 &OnTxTimeout, // txTimeout
GregCr 1:22e02d1cfbca 56 &OnRxTimeout, // rxTimeout
GregCr 1:22e02d1cfbca 57 &OnRxError, // rxError
GregCr 1:22e02d1cfbca 58 NULL, // rangingDone
GregCr 1:22e02d1cfbca 59 NULL, // cadDone
GregCr 1:22e02d1cfbca 60 };
GregCr 1:22e02d1cfbca 61
FCH_31 2:57f098de07c7 62 // mosi, miso, sclk, nss, busy, dio1, dio2, dio3, rst, callbacks...
FCH_31 2:57f098de07c7 63 SX1280Hal Radio ( D11 , D12 , D13 , D7 , D3 , D5 , NC , NC , A0 , &callbacks );
GregCr 1:22e02d1cfbca 64
FCH_31 3:3adb454ba1d2 65 DigitalOut ANT_SW ( A3 ) ;
FCH_31 3:3adb454ba1d2 66 DigitalOut TxLed ( A4 ) ;
FCH_31 3:3adb454ba1d2 67 DigitalOut RxLed ( A5 ) ;
FCH_31 3:3adb454ba1d2 68 DigitalOut F_CS ( D6 ) ; // MBED description of pin
FCH_31 3:3adb454ba1d2 69 DigitalOut SD_CS ( D8 ) ; // MBED description of pin
GregCr 1:22e02d1cfbca 70
FCH_31 2:57f098de07c7 71 #define TX_TIMEOUT_VALUE 100 // ms Number of tick size steps for tx timeout
FCH_31 3:3adb454ba1d2 72 #define RX_TIMEOUT_VALUE 100 // ms Number of tick size steps for rx timeout
FCH_31 2:57f098de07c7 73 #define RX_TIMEOUT_TICK_SIZE RADIO_TICK_SIZE_1000_US // Size of ticks (used for Tx and Rx timeout)
FCH_31 2:57f098de07c7 74
FCH_31 2:57f098de07c7 75 uint16_t RxIrqMask = IRQ_RX_DONE | IRQ_RX_TX_TIMEOUT; // Mask of IRQs to listen to in rx mode
FCH_31 2:57f098de07c7 76 uint16_t TxIrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT; // Mask of IRQs to listen to in tx mode
FCH_31 2:57f098de07c7 77
FCH_31 2:57f098de07c7 78 PacketParams_t PacketParams; // Locals parameters and status for radio API
FCH_31 2:57f098de07c7 79 PacketStatus_t PacketStatus; // NEED TO BE OPTIMIZED, COPY OF STUCTURE ALREADY EXISTING
GregCr 1:22e02d1cfbca 80
FCH_31 2:57f098de07c7 81 ModulationParams_t modulationParams;
FCH_31 2:57f098de07c7 82
FCH_31 2:57f098de07c7 83
FCH_31 4:41a92fbf702a 84 /* Instantiate the expansion board */
FCH_31 4:41a92fbf702a 85 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
FCH_31 4:41a92fbf702a 86
FCH_31 4:41a92fbf702a 87 /* Retrieve the composing elements of the expansion board */
FCH_31 4:41a92fbf702a 88 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
FCH_31 4:41a92fbf702a 89 static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
FCH_31 4:41a92fbf702a 90 static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
FCH_31 4:41a92fbf702a 91 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
FCH_31 4:41a92fbf702a 92 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
FCH_31 4:41a92fbf702a 93
FCH_31 4:41a92fbf702a 94 uint8_t id1, id2, id3, id4, id5, id6, id7;
FCH_31 4:41a92fbf702a 95 float valueTH1, valueTH2, valueTP1, valueTP2;
FCH_31 4:41a92fbf702a 96 char buffer1[32], buffer2[32], buffer3[32], buffer4[32];
FCH_31 4:41a92fbf702a 97 int32_t axesA[3], axesM[3], axesG1[3], axesG2[3];
FCH_31 4:41a92fbf702a 98
FCH_31 4:41a92fbf702a 99 static char *print_double(char* str, double v, int decimalDigits=2) ;
FCH_31 4:41a92fbf702a 100 void baud ( int baudrate ) ;
GregCr 1:22e02d1cfbca 101
FCH_31 2:57f098de07c7 102 void setup () {
FCH_31 3:3adb454ba1d2 103 baud (115200);
FCH_31 2:57f098de07c7 104 printf( "\n\n\r SX1280 Terminal IoT LORA MODULATION 2.4GHz (%s)\n\n\r", FIRMWARE_VERSION );
GregCr 1:22e02d1cfbca 105
FCH_31 2:57f098de07c7 106 F_CS = 1 ;
FCH_31 2:57f098de07c7 107 SD_CS = 1 ;
FCH_31 2:57f098de07c7 108 RxLed = 1 ;
FCH_31 2:57f098de07c7 109 TxLed = 1 ;
FCH_31 2:57f098de07c7 110 ANT_SW = 1 ;
GregCr 1:22e02d1cfbca 111
GregCr 1:22e02d1cfbca 112 wait_ms( 500 ); // wait for on board DC/DC start-up time
GregCr 1:22e02d1cfbca 113
FCH_31 2:57f098de07c7 114 Radio.Init ( );
FCH_31 2:57f098de07c7 115 Radio.SetRegulatorMode ( USE_DCDC ); // Can also be set in LDO mode but consume more power
GregCr 1:22e02d1cfbca 116
FCH_31 2:57f098de07c7 117 memset ( &Buffer, 0x00, BufferSize );
GregCr 1:22e02d1cfbca 118
FCH_31 2:57f098de07c7 119
FCH_31 2:57f098de07c7 120 RxLed = 0;
FCH_31 2:57f098de07c7 121 TxLed = 0;
FCH_31 2:57f098de07c7 122
FCH_31 2:57f098de07c7 123 modulationParams.PacketType = PACKET_TYPE_LORA ;
FCH_31 2:57f098de07c7 124 modulationParams.Params.LoRa.SpreadingFactor = LORA_SF7 ;
FCH_31 2:57f098de07c7 125 modulationParams.Params.LoRa.Bandwidth = LORA_BW_0400 ;
FCH_31 2:57f098de07c7 126 modulationParams.Params.LoRa.CodingRate = LORA_CR_4_5 ;
GregCr 1:22e02d1cfbca 127
FCH_31 2:57f098de07c7 128 PacketParams.PacketType = PACKET_TYPE_LORA ;
FCH_31 2:57f098de07c7 129 PacketParams.Params.LoRa.PreambleLength = 0x08 ;
FCH_31 2:57f098de07c7 130 PacketParams.Params.LoRa.HeaderType = LORA_PACKET_VARIABLE_LENGTH;
FCH_31 3:3adb454ba1d2 131 PacketParams.Params.LoRa.PayloadLength = 34 ;
FCH_31 3:3adb454ba1d2 132 PacketParams.Params.LoRa.CrcMode = LORA_CRC_ON ;
FCH_31 3:3adb454ba1d2 133 PacketParams.Params.LoRa.InvertIQ = LORA_IQ_INVERTED ;
GregCr 1:22e02d1cfbca 134
FCH_31 2:57f098de07c7 135 Radio.SetStandby ( STDBY_RC );
FCH_31 2:57f098de07c7 136 Radio.SetPacketType ( modulationParams.PacketType );
FCH_31 2:57f098de07c7 137 Radio.SetModulationParams ( &modulationParams );
FCH_31 2:57f098de07c7 138 Radio.SetPacketParams ( &PacketParams );
FCH_31 2:57f098de07c7 139 Radio.SetRfFrequency ( RF_FREQUENCY );
FCH_31 2:57f098de07c7 140 Radio.SetBufferBaseAddresses ( 0x00, 0x00 );
FCH_31 2:57f098de07c7 141 Radio.SetTxParams ( TX_OUTPUT_POWER, RADIO_RAMP_20_US );
FCH_31 2:57f098de07c7 142 Radio.SetDioIrqParams ( RxIrqMask, RxIrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
FCH_31 2:57f098de07c7 143 Radio.SetRx ( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, RX_TIMEOUT_VALUE } );
GregCr 1:22e02d1cfbca 144
FCH_31 4:41a92fbf702a 145 AppState = APP_LOWPOWER;
FCH_31 2:57f098de07c7 146
FCH_31 4:41a92fbf702a 147 /* Enable all sensors */
FCH_31 4:41a92fbf702a 148 hum_temp->enable ();
FCH_31 4:41a92fbf702a 149 press_temp->enable ();
FCH_31 4:41a92fbf702a 150 magnetometer->enable ();
FCH_31 4:41a92fbf702a 151 accelerometer->enable ();
FCH_31 4:41a92fbf702a 152 acc_gyro->enable_x ();
FCH_31 4:41a92fbf702a 153 acc_gyro->enable_g ();
FCH_31 4:41a92fbf702a 154
FCH_31 4:41a92fbf702a 155 printf("\r\n--- Starting new run ---\r\n");
FCH_31 4:41a92fbf702a 156
FCH_31 4:41a92fbf702a 157 hum_temp->read_id (&id1);
FCH_31 4:41a92fbf702a 158 printf("HTS221 humidity & temperature = 0x%X\r\n", id1);
FCH_31 4:41a92fbf702a 159 press_temp->read_id (&id2);
FCH_31 4:41a92fbf702a 160 printf("LPS22HB pressure & temperature = 0x%X\r\n", id2);
FCH_31 4:41a92fbf702a 161 magnetometer->read_id (&id3);
FCH_31 4:41a92fbf702a 162 printf("LSM303AGR magnetometer = 0x%X\r\n", id3);
FCH_31 4:41a92fbf702a 163 accelerometer->read_id (&id4);
FCH_31 4:41a92fbf702a 164 printf("LSM303AGR accelerometer = 0x%X\r\n", id4);
FCH_31 4:41a92fbf702a 165 acc_gyro->read_id (&id5);
FCH_31 4:41a92fbf702a 166 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id5);
FCH_31 4:41a92fbf702a 167
FCH_31 4:41a92fbf702a 168 }
GregCr 1:22e02d1cfbca 169
FCH_31 2:57f098de07c7 170 void loop () {
FCH_31 4:41a92fbf702a 171 wait(4);
FCH_31 4:41a92fbf702a 172
FCH_31 4:41a92fbf702a 173 F_CS = 1 ;
FCH_31 4:41a92fbf702a 174 SD_CS = 1 ;
FCH_31 4:41a92fbf702a 175 RxLed = 1 ;
FCH_31 4:41a92fbf702a 176 TxLed = 1 ;
FCH_31 4:41a92fbf702a 177 ANT_SW = 1 ;
FCH_31 4:41a92fbf702a 178
FCH_31 4:41a92fbf702a 179 wait( 0.5 ); // wait for on board DC/DC start-up time
FCH_31 4:41a92fbf702a 180
FCH_31 4:41a92fbf702a 181 /* Enable all sensors */
FCH_31 4:41a92fbf702a 182 hum_temp->enable ();
FCH_31 4:41a92fbf702a 183 press_temp->enable ();
FCH_31 4:41a92fbf702a 184 magnetometer->enable ();
FCH_31 4:41a92fbf702a 185 accelerometer->enable ();
FCH_31 4:41a92fbf702a 186 acc_gyro->enable_x ();
FCH_31 4:41a92fbf702a 187 acc_gyro->enable_g ();
FCH_31 4:41a92fbf702a 188
FCH_31 4:41a92fbf702a 189 //wait_ms( 500 ); // wait for on board DC/DC start-up time
FCH_31 4:41a92fbf702a 190
FCH_31 4:41a92fbf702a 191 wait(0.5);
FCH_31 4:41a92fbf702a 192 printf("============================================================== \r\n");
FCH_31 4:41a92fbf702a 193
FCH_31 4:41a92fbf702a 194 hum_temp->get_temperature (&valueTH1) ; wait_ms( 10 );
FCH_31 4:41a92fbf702a 195 hum_temp->get_humidity (&valueTH2) ; wait_ms( 10 );
FCH_31 4:41a92fbf702a 196
FCH_31 4:41a92fbf702a 197 printf("HTS221 : [temp] %7s C, [hum] %s%%\r\n" , print_double(buffer1, valueTH1), print_double(buffer2, valueTH2));
FCH_31 4:41a92fbf702a 198
FCH_31 4:41a92fbf702a 199 press_temp->get_temperature (&valueTP1) ;
FCH_31 4:41a92fbf702a 200 press_temp->get_pressure (&valueTP2) ;
FCH_31 4:41a92fbf702a 201 printf("LPS22HB : [temp] %7s C, [press] %s mbar\r\n" , print_double(buffer3, valueTP1), print_double(buffer4, valueTP2));
FCH_31 4:41a92fbf702a 202
FCH_31 4:41a92fbf702a 203 magnetometer->get_m_axes (axesM) ;
FCH_31 4:41a92fbf702a 204 printf("LSM303AGR : [mag/mgauss] %6ld, %6ld, %6ld\r\n", axesM[0], axesM[1], axesM[2]);
FCH_31 4:41a92fbf702a 205
FCH_31 4:41a92fbf702a 206 accelerometer->get_x_axes (axesA) ;
FCH_31 4:41a92fbf702a 207 printf("LSM303AGR : [acc/mg] %6ld, %6ld, %6ld\r\n", axesA[0], axesA[1], axesA[2]);
FCH_31 4:41a92fbf702a 208
FCH_31 4:41a92fbf702a 209 acc_gyro->get_x_axes (axesG1) ;
FCH_31 4:41a92fbf702a 210 acc_gyro->get_g_axes (axesG2) ;
FCH_31 4:41a92fbf702a 211 printf("LSM6DSL : [acc/mg] %6ld, %6ld, %6ld\r\n", axesG1[0], axesG1[1], axesG1[2]);
FCH_31 4:41a92fbf702a 212 printf("LSM6DSL : [gyro/mdps] %6ld, %6ld, %6ld\r\n", axesG2[0], axesG2[1], axesG2[2]);
FCH_31 4:41a92fbf702a 213
FCH_31 4:41a92fbf702a 214 printf("============================================================== \r\n");
FCH_31 4:41a92fbf702a 215
FCH_31 4:41a92fbf702a 216 /* Disable all sensors */
FCH_31 4:41a92fbf702a 217 hum_temp->disable ();
FCH_31 4:41a92fbf702a 218 press_temp->disable ();
FCH_31 4:41a92fbf702a 219 magnetometer->disable ();
FCH_31 4:41a92fbf702a 220 accelerometer->disable ();
FCH_31 4:41a92fbf702a 221 acc_gyro->disable_x ();
FCH_31 4:41a92fbf702a 222 acc_gyro->disable_g ();
FCH_31 4:41a92fbf702a 223
FCH_31 4:41a92fbf702a 224
FCH_31 4:41a92fbf702a 225
FCH_31 3:3adb454ba1d2 226 TxLed = 1 ;
FCH_31 4:41a92fbf702a 227 //int n = sprintf ( Message, "MSG DATA IOT TERMINAL: %d", cptr++) ;
FCH_31 4:41a92fbf702a 228
FCH_31 4:41a92fbf702a 229 memset ( &Buffer , 0x00, BufferSize );
FCH_31 4:41a92fbf702a 230 memset ( &Message, 0x00, BufferSize );
FCH_31 4:41a92fbf702a 231
FCH_31 4:41a92fbf702a 232 int n = sprintf ( Message, "%1s %1s %s %s %s&",
FCH_31 4:41a92fbf702a 233 ID_TERMINAL,
FCH_31 4:41a92fbf702a 234 ID_GATEWAY,
FCH_31 4:41a92fbf702a 235 print_double(buffer1, valueTH1),
FCH_31 4:41a92fbf702a 236 print_double(buffer2, valueTH2),
FCH_31 4:41a92fbf702a 237 print_double(buffer3, valueTP2) ) ;
FCH_31 4:41a92fbf702a 238
FCH_31 3:3adb454ba1d2 239 memcpy ( Buffer , Message , MESSAGESIZE );
FCH_31 3:3adb454ba1d2 240 printf ( "*** TERM *** Message = %s \r\n", Buffer );
FCH_31 3:3adb454ba1d2 241 Radio.SetDioIrqParams ( TxIrqMask , TxIrqMask , IRQ_RADIO_NONE, IRQ_RADIO_NONE );
FCH_31 3:3adb454ba1d2 242 Radio.SendPayload ( Buffer , BufferSize,( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, TX_TIMEOUT_VALUE } );
FCH_31 3:3adb454ba1d2 243 TxLed = 0 ;
FCH_31 4:41a92fbf702a 244
FCH_31 4:41a92fbf702a 245
GregCr 1:22e02d1cfbca 246 }
FCH_31 3:3adb454ba1d2 247
FCH_31 2:57f098de07c7 248
GregCr 1:22e02d1cfbca 249
FCH_31 2:57f098de07c7 250 int main( ) { setup () ; while (1) loop () ; }
GregCr 1:22e02d1cfbca 251
FCH_31 3:3adb454ba1d2 252 void OnTxDone ( void ) { /*printf( "*** TERM *** OnTxDone \r\n" ); */ AppState = APP_TX ; }
FCH_31 2:57f098de07c7 253 void OnRxDone ( void ) { printf( "*** TERM *** OnRxDone \r\n" ); AppState = APP_RX ; }
FCH_31 2:57f098de07c7 254 void OnTxTimeout ( void ) { printf( "*** TERM *** OnTxTimeout \r\n" ); AppState = APP_TX_TIMEOUT ; }
FCH_31 2:57f098de07c7 255 void OnRxTimeout ( void ) { printf( "*** TERM *** OnRxTimeout \r\n" ); AppState = APP_RX_TIMEOUT ; }
FCH_31 2:57f098de07c7 256 void OnRxError ( IrqErrorCode_t errorCode ) { printf( "*** TERM *** OnRxError \r\n" ); AppState = APP_RX_ERROR ; }
FCH_31 2:57f098de07c7 257 void OnRangingDone ( IrqRangingCode_t val ) { printf( "*** TERM *** OnRangingDone \r\n" ); }
FCH_31 2:57f098de07c7 258 void OnCadDone ( bool channelActivityDetected ) { printf( "*** TERM *** OnCadDone \r\n" ); }
FCH_31 2:57f098de07c7 259
FCH_31 2:57f098de07c7 260 /*
GregCr 1:22e02d1cfbca 261 Radio.GetPacketStatus(&packetStatus);
GregCr 1:22e02d1cfbca 262 RssiValue = packetStatus.Lr24.RssiPkt;
GregCr 1:22e02d1cfbca 263 SnrValue = packetStatus.Lr24.SnrPkt;
GregCr 1:22e02d1cfbca 264 printf("rssi: %d; snr: %d\n\r", RssiValue, SnrValue );
FCH_31 2:57f098de07c7 265 */
FCH_31 4:41a92fbf702a 266
FCH_31 4:41a92fbf702a 267
FCH_31 4:41a92fbf702a 268
FCH_31 4:41a92fbf702a 269 /* Helper function for printing floats & doubles */
FCH_31 4:41a92fbf702a 270 static char *print_double(char* str, double v, int decimalDigits)
FCH_31 4:41a92fbf702a 271 {
FCH_31 4:41a92fbf702a 272 int i = 1;
FCH_31 4:41a92fbf702a 273 int intPart, fractPart;
FCH_31 4:41a92fbf702a 274 int len;
FCH_31 4:41a92fbf702a 275 char *ptr;
FCH_31 4:41a92fbf702a 276
FCH_31 4:41a92fbf702a 277 /* prepare decimal digits multiplicator */
FCH_31 4:41a92fbf702a 278 for (;decimalDigits!=0; i*=10, decimalDigits--);
FCH_31 4:41a92fbf702a 279
FCH_31 4:41a92fbf702a 280 /* calculate integer & fractinal parts */
FCH_31 4:41a92fbf702a 281 intPart = (int)v;
FCH_31 4:41a92fbf702a 282 fractPart = (int)((v-(double)(int)v)*i);
FCH_31 4:41a92fbf702a 283
FCH_31 4:41a92fbf702a 284 /* fill in integer part */
FCH_31 4:41a92fbf702a 285 sprintf(str, "%i.", intPart);
FCH_31 4:41a92fbf702a 286
FCH_31 4:41a92fbf702a 287 /* prepare fill in of fractional part */
FCH_31 4:41a92fbf702a 288 len = strlen(str);
FCH_31 4:41a92fbf702a 289 ptr = &str[len];
FCH_31 4:41a92fbf702a 290
FCH_31 4:41a92fbf702a 291 /* fill in leading fractional zeros */
FCH_31 4:41a92fbf702a 292 for (i/=10;i>1; i/=10, ptr++) {
FCH_31 4:41a92fbf702a 293 if (fractPart >= i) {
FCH_31 4:41a92fbf702a 294 break;
FCH_31 4:41a92fbf702a 295 }
FCH_31 4:41a92fbf702a 296 *ptr = '0';
FCH_31 4:41a92fbf702a 297 }
FCH_31 4:41a92fbf702a 298
FCH_31 4:41a92fbf702a 299 /* fill in (rest of) fractional part */
FCH_31 4:41a92fbf702a 300 sprintf(ptr, "%i", fractPart);
FCH_31 4:41a92fbf702a 301
FCH_31 4:41a92fbf702a 302 return str;
FCH_31 4:41a92fbf702a 303 }
FCH_31 4:41a92fbf702a 304
FCH_31 4:41a92fbf702a 305
FCH_31 4:41a92fbf702a 306
FCH_31 4:41a92fbf702a 307 /* Specify serial datarate for UART debug output */
FCH_31 4:41a92fbf702a 308 void baud ( int baudrate ) { Serial s( USBTX, USBRX ); s.baud( baudrate );}