experimental_TX

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <SPI.h>
#include <RH_RF95.h>

/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
   which provides a common 'type' for sensor data and some helper functions.

   To use this driver you will also need to download the Adafruit_Sensor
   library and include it in your libraries folder.

   You should also assign a unique ID to this sensor for use with
   the Adafruit Sensor API so that you can identify this particular
   sensor in any data logs, etc.  To assign a unique ID, simply
   provide an appropriate value in the constructor below (12345
   is used by default in this example).

   Connections
   ===========
   Connect SCL to analog 5
   Connect SDA to analog 4
   Connect VDD to 3-5V DC
   Connect GROUND to common ground

   History
   =======
   2015/MAR/03  - First release (KTOWN)
   2015/AUG/27  - Added calibration and system status helpers
*/

/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (10)
RH_RF95 rf95(8, 3); // Adafruit Feather M0 with RFM95

uint8_t calibData[11] = {-13,-25,-13,-2,0,0,-93,276,306,1000,716};

Adafruit_BNO055 bno = Adafruit_BNO055(55);
int led = 9;
/**************************************************************************/
/*
    Displays some basic information on this sensor from the unified
    sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
  sensor_t sensor;
  bno.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display some basic info about the sensor status
*/
/**************************************************************************/
void displaySensorStatus(void)
{
  /* Get the system status values (mostly for debugging purposes) */
  uint8_t system_status, self_test_results, system_error;
  system_status = self_test_results = system_error = 0;
  bno.getSystemStatus(&system_status, &self_test_results, &system_error);

  /* Display the results in the Serial Monitor */
  Serial.println("");
  Serial.print("System Status: 0x");
  Serial.println(system_status, HEX);
  Serial.print("Self Test:     0x");
  Serial.println(self_test_results, HEX);
  Serial.print("System Error:  0x");
  Serial.println(system_error, HEX);
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display sensor calibration status
*/
/**************************************************************************/
void displayCalStatus(void)
{
  /* Get the four calibration values (0..3) */
  /* Any sensor data reporting 0 should be ignored, */
  /* 3 means 'fully calibrated" */
  uint8_t system, gyro, accel, mag;
  system = gyro = accel = mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);

  /* The data should be ignored until the system calibration is > 0 */
  Serial.print("\t");
  if (!system)
  {
    Serial.print("! ");
  }

  /* Display the individual values */
  Serial.print("Sys:");
  Serial.print(system, DEC);
  Serial.print(" G:");
  Serial.print(gyro, DEC);
  Serial.print(" A:");
  Serial.print(accel, DEC);
  Serial.print(" M:");
  Serial.print(mag, DEC);
}

/**************************************************************************/
/*
    dtostrf emulation for Feather M0
*/
/**************************************************************************/
char *dtostrf(double value, int width, unsigned int precision, char *result)
{
  int decpt, sign, reqd, pad;
  const char *s, *e;
  char *p;
  s = fcvt(value, precision, &decpt, &sign);
  if (precision == 0 && decpt == 0) {
  s = (*s < '5') ? "0" : "1";
    reqd = 1;
  } else {
    reqd = strlen(s);
    if (reqd > decpt) reqd++;
    if (decpt == 0) reqd++;
  }
  if (sign) reqd++;
  p = result;
  e = p + reqd;
  pad = width - reqd;
  if (pad > 0) {
    e += pad;
    while (pad-- > 0) *p++ = ' ';
  }
  if (sign) *p++ = '-';
  if (decpt <= 0 && precision > 0) {
    *p++ = '0';
    *p++ = '.';
    e++;
    while ( decpt < 0 ) {
      decpt++;
      *p++ = '0';
    }
  }
  while (p < e) {
    *p++ = *s++;
    if (p == e) break;
    if (--decpt == 0) *p++ = '.';
  }
  if (width < 0) {
    pad = (reqd + width) * -1;
    while (pad-- > 0) *p++ = ' ';
  }
  *p = 0;
  return result;
}
/**************************************************************************/
/*
    Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Test"); Serial.println("");
 pinMode(led, OUTPUT);
 // pinMode(RFM95_RST, OUTPUT);
  //digitalWrite(RFM95_RST, HIGH);
 // Serial.begin(9600);
  //while (!Serial) ; // Wait for serial port to be available
  if (!rf95.init())
    Serial.println("init failed");
  // Defaults after init are 434.0MHz, 13dBm, Bw = 125 kHz, Cr = 4/5, Sf = 128chips/symbol, CRC on

rf95.setModemConfig(RH_RF95::Bw500Cr45Sf128);
rf95.setFrequency(915.0);
rf95.setTxPower(23, false);

  // The default transmitter power is 13dBm, using PA_BOOST.
  // If you are using RFM95/96/97/98 modules which uses the PA_BOOST transmitter pin, then
  // you can set transmitter powers from 5 to 23 dBm:
//  driver.setTxPower(23, false);
  // If you are using Modtronix inAir4 or inAir9,or any other module which uses the
  // transmitter RFO pins and not the PA_BOOST pins
  // then you can configure the power transmitter power for -1 to 14 dBm and with useRFO true.
  // Failure to do that will result in extremely low transmit powers.
//  driver.setTxPower(14, true);

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  delay(1000);

  /* Display some basic information on this sensor */
  displaySensorDetails();

  /* Optional: Display current status */
  displaySensorStatus();
  bno.setSensorOffsets(calibData);
  bno.setExtCrystalUse(true);
}

/**************************************************************************/
/*
    Arduino loop function, called once 'setup' is complete (your own code
    should go here)
*/
/**************************************************************************/
void loop(void)
{
  /* Get a new sensor event */
  sensors_event_t event;
  bno.getEvent(&event);
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  imu::Vector<3> acceler = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
  imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

    uint8_t systemstatus, gyrostatus, accelstatus, mag;
  systemstatus = gyrostatus = accelstatus = mag = 0;
  bno.getCalibration(&systemstatus, &gyrostatus, &accelstatus, &mag);

  /* Display the floating point data */

  Serial.print(euler.x(), 4);
  Serial.print(" ");
  Serial.print(euler.y(), 4);
  Serial.print(" ");
  Serial.print(euler.z(), 4);
  Serial.print(" ");
  Serial.print(acceler.x(), 4);
  Serial.print(" ");
  Serial.print(acceler.y(), 4);
  Serial.print(" ");
  Serial.print(acceler.z(), 4);
  Serial.print(" ");
  Serial.print( gyro.x(), 4);
  Serial.print(" ");
  Serial.print( gyro.y(), 4);
  Serial.print(" ");
  Serial.println( gyro.z(), 4);


//Character Array
char xVal[9]; //9 is the maximum number of characters expected in an x-value (i.e. xxx.yyyy\0 = 9 chars)
                //(including the '.', '-' and invisible end-of-string indicator '\0' <- this is a special marker just counts as 1 char)
  char yVal[10];//10 is the maximum number of characters expected in an x-value (i.e. -xxx.yyyy\0 = 10 chars)
                //(including the '.', '-' and invisible end-of-string indicator '\0' <- this is a special marker just counts as 1 char)
  char zVal[10]; //same as y

  char axVal[10];
  char ayVal[10];
  char azVal[10];

  char gxVal[10];
  char gyVal[10];
  char gzVal[10];

  //Convert the floating points to strings.
  dtostrf(euler.x(),3,4,xVal);
  dtostrf(euler.y(),3,4,yVal);
  dtostrf(euler.z(),3,4,zVal);

  dtostrf(acceler.x(),3,4,axVal);
  dtostrf(acceler.y(),3,4,ayVal);
  dtostrf(acceler.z(),3,4,azVal);

  dtostrf(gyro.x(),3,4,gxVal);
  dtostrf(gyro.y(),3,4,gyVal);
  dtostrf(gyro.z(),3,4,gzVal);

  char payload[RH_RF95_MAX_MESSAGE_LEN]; //This will hold what we'll actually transmit.  32 is the maximum number of characters expected in the this will is the string

  sprintf(payload,"%s:%s:%s:%s:%s:%s:%s:%s:%s:%d:%d:%d",xVal,yVal,zVal,axVal,ayVal,azVal,gxVal,gyVal,gzVal,systemstatus,accelstatus,gyrostatus);



  /* Optional: Display sensor status (debug only) */
  //displaySensorStatus();


  /* New line for the next sample */
 //Serial.println(payload);
// if (rf95.available())
 // {
  //  Serial.println("test");
    // Should be a message for us now
    uint8_t buf[RH_RF95_MAX_MESSAGE_LEN];
    uint8_t len = sizeof(buf);
   // if (rf95.recv(buf, &len))
  //  {
      digitalWrite(led, HIGH);
//      RH_RF95::printBuffer("request: ", buf, len);
   //   Serial.print("got request: ");
   //   Serial.println((char*)buf);
   //   Serial.print("RSSI: ");
   //   Serial.println(rf95.lastRssi(), DEC);

      // Send a reply
     // uint8_t data[] = "And hello back to you";


     //Sends data over
      rf95.send((uint8_t*)payload, sizeof(payload));
      rf95.waitPacketSent();
   //   Serial.println("Sent a reply");
       digitalWrite(led, LOW);
    //}
    //else
    //{
      //Serial.println("recv failed");
    //}
  //}

  /* Wait the specified delay before requesting nex data */
  delay(BNO055_SAMPLERATE_DELAY_MS);
}