example_main_all_sensors.cpp

 
#include <memory>
 
#include "sensesp.h"
#include "sensesp/sensors/sensor.h"
#include "sensesp/signalk/signalk_output.h"
#include "sensesp/system/lambda_consumer.h"
#include "sensesp_app_builder.h"
 
#include "sensesp/sensors/digital_input.h"
#include "sensesp/transforms/debounce.h"
 
#include "sensesp/transforms/angle_correction.h"
#include "sensesp/transforms/curveinterpolator.h"
 
#include "sensesp/signalk/signalk_types.h"
 
#include "sensesp/transforms/linear.h"
 
#include "orientation_sensor.h"
#include "signalk_orientation.h"
 
 
// Sensor hardware details: I2C addresses and pins       
#define BOARD_ACCEL_MAG_I2C_ADDR    (0x1F) 
#define BOARD_GYRO_I2C_ADDR         (0x21) 
#define PIN_I2C_SDA (23)          // Adjust to your board. A value of -1
#define PIN_I2C_SCL (25)          //   will use default Arduino pins.
#define PIN_SWITCH_CAL_SAVE (32)  // Optional switch attached to this pin saves magnetic calibration
#define SWITCH_ACTIVE_STATE (0)   // Input is LOW when Switch is pushed
 
// How often orientation parameters are published via Signal K message
// If a report interval is saved for a particular sensor path (via the web
// interface), that overrides the following #define for that report.
#define ORIENTATION_REPORTING_INTERVAL_MS (100)
 
using namespace sensesp;
 
class DeviationInterpolator : public CurveInterpolator {
 public:
  DeviationInterpolator(String config_path = "")
      : CurveInterpolator( NULL, config_path) {
    //default Deviation table is straight 1:1 conversion
    //   clear_samples();
    //   add_sample(CurveInterpolator::Sample(0.0, 0.0));
    //   add_sample(CurveInterpolator::Sample(6.3, 6.3));  
  }
};
 
 
void setup() {
  //without following, get runtime messages:
  // I (2147) ARDUINO: LEDC attached to pin 2 (channel 0, resolution 8)
  // E (2149) ARDUINO: IO 0 is not set as GPIO. Execute digitalMode(0, OUTPUT) first.
  // E (2149) ARDUINO: IO 2 is not set as GPIO. Execute digitalMode(2, OUTPUT) first.
  // E (2149) ARDUINO: IO 4 is not set as GPIO. Execute digitalMode(4, OUTPUT) first.
  pinMode(0, OUTPUT); 
  pinMode(2, OUTPUT); //this doesn't prevent the GPIO2 complaint.
  pinMode(4, OUTPUT);
 
  SetupLogging(ESP_LOG_INFO); //inits the Serial port and starts logging output
 
  SensESPAppBuilder builder;
  sensesp_app = (&builder)
                    // Set a custom hostname for the app.
                    ->set_hostname("eCompass")
                    // Optionally, hard-code the WiFi and Signal K server
                    // settings. They can also be configured using the web interface.
                    //->set_wifi_client("My WiFi SSID", "my_wifi_password")
                    //->set_wifi_access_point("My AP SSID", "my_ap_password")
                    //->set_sk_server("10.0.0.27", 3000)
                    //SensESP has several builtin sensors, e.g. freemem, uptime, IP address
                    //  Optionally enable them here to output their values in SK reports.
                    ->enable_uptime_sensor()
                    ->enable_ip_address_sensor()
                    ->enable_free_mem_sensor()
                    ->enable_system_hz_sensor()
                    //->enable_wifi_signal_sensor()
                    ->get_app();
 
  const char* kSKPathHeadingCompass  = "navigation.headingCompass";
  const char* kSKPathHeadingMagnetic = "navigation.headingMagnetic";
  const char* kSKPathAttitude        = "navigation.attitude";
  const char* kSKPathTurnRate    = "navigation.rateOfTurn";
  const char* kSKPathRollRate    = "navigation.rateOfRoll";
  const char* kSKPathPitchRate   = "navigation.rateOfPitch";
  const char* kSKPathTemperature =
                 "environment.inside.ecompass.temperature";
  const char* kSKPathAccelX       = "sensors.accelerometer.accel_x"; 
  const char* kSKPathAccelY       = "sensors.accelerometer.accel_y"; 
  const char* kSKPathAccelZ       = "sensors.accelerometer.accel_z"; 
  const char* kSKPathMagFit         = "orientation.calibration.magfit";
  const char* kSKPathMagFitTrial    = "orientation.calibration.magfittrial";
  const char* kSKPathMagSolver      = "orientation.calibration.magsolver";
  const char* kSKPathMagInclination = "orientation.calibration.maginclination";
  const char* kSKPathMagBValue      = "orientation.calibration.magmagnitude";
  const char* kSKPathMagBValueTrial = "orientation.calibration.magmagnitudetrial";
  const char* kSKPathMagNoise       = "orientation.calibration.magnoise";
  const char* kSKPathMagCalValues   = "orientation.calibration.magvalues";
 
  const char* kConfigPathNone = ""; 
 
  auto* orientation_sensor = new OrientationSensor(
      PIN_I2C_SDA, PIN_I2C_SCL, BOARD_ACCEL_MAG_I2C_ADDR, BOARD_GYRO_I2C_ADDR);
  const int fusionIntervalMs = 1000 / orientation_sensor->GetFusionRateHz();
  event_loop()->onRepeat( fusionIntervalMs,
                          [orientation_sensor]() { orientation_sensor->ReadAndProcessSensors(); }
                        );
  // Create sensor for compass heading output     
  auto* sensor_heading = new OrientationValues(
      orientation_sensor, OrientationValues::kCompassHeading);
  auto compass_heading = std::make_shared<RepeatSensor<float>>(
      ORIENTATION_REPORTING_INTERVAL_MS,
      [sensor_heading]() { return sensor_heading->ReportValue(); }
    );
  /* RepeatSensor() is not defined as a configurable class, so we can't
     use the web UI to update the reporting rate.  TODO - expand RepeatSensor
     class definition.
   ConfigItem(compass_heading)
    ->set_title("Heading Report Rate")
    ->set_description("interval (ms) between Compass and Magnetic Heading outputs")
    ->set_sort_order(200);
    */
  auto compass_sk_output = std::make_shared<SKOutput<float>>(
      kSKPathHeadingCompass,    // Signal K path
      kConfigPathNone           // configuration path
      );
 
  auto magneticheading_sk_output = std::make_shared<SKOutput<float>>(
      kSKPathHeadingMagnetic,   // Signal K path
      kConfigPathNone           // configuration path
                                // metadata
      );
  const char* kConfigPathDeviation = "/sensors/hdg/deviation";
  auto* deviationInterpolator = new DeviationInterpolator(kConfigPathDeviation);
  ConfigItem(deviationInterpolator)
    ->set_title("Deviation Table")
    ->set_description("Interpolation Values")
    ->set_sort_order(1001);
 
  //Add an AngleCorrection transform, to adjust for any mounting offsets
  //  Pi/2 rotation in this example
  const char* kConfigPathHeadingOffset = "/sensors/hdg/offset";
  auto* mountingOffset = new AngleCorrection((PI/2.0), 0.0, kConfigPathHeadingOffset);
  ConfigItem(mountingOffset)
    ->set_title("Mounting Offset")
    ->set_description("Enter any adjustment to be applied to all headings (e.g. from mounting offsets)")
    ->set_sort_order(400);
 
  // Connect compass output to Signal K transforms and outputs
  compass_heading
    ->connect_to(mountingOffset)
    ->connect_to(compass_sk_output)
    ->connect_to(deviationInterpolator)
    ->connect_to(new AngleCorrection(0.0, 0.0, kConfigPathNone))  // Normalize to [0..2Pi] after interpolation
    ->connect_to(magneticheading_sk_output);
 
  auto* sensor_roll = new OrientationValues(
      orientation_sensor, OrientationValues::kRoll);
  auto* sensor_pitch = new OrientationValues(
      orientation_sensor, OrientationValues::kPitch);
  auto* sensor_yaw = new OrientationValues(
      orientation_sensor, OrientationValues::kYaw);
 
  auto attitude_sensor = std::make_shared<RepeatSensor<AttitudeVector>>(
      ORIENTATION_REPORTING_INTERVAL_MS,
      [sensor_roll, sensor_pitch, sensor_yaw]() 
      { return AttitudeVector(sensor_roll->ReportValue(),
                              sensor_pitch->ReportValue(), 
                              sensor_yaw->ReportValue()
                            );
      }
    );
  auto attitude_sk_output = std::make_shared<SKOutput<AttitudeVector>>(
      kSKPathAttitude,    // Signal K path
      kConfigPathNone    // configuration path
  );
  attitude_sensor->connect_to(attitude_sk_output);
 
  // Create output for Magnetic Calibration Fit    
  auto* sensor_magcalfit = new OrientationValues(
      orientation_sensor, OrientationValues::kMagCalFitInUse);
  auto magcalfit = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_magcalfit]() { return sensor_magcalfit->ReportValue(); });
  // Need to provide metadata, as Mag Cal related values are not defined in the Signal K spec.
  auto magcalfit_metadata = std::make_shared<SKMetadata>();
  magcalfit_metadata->units_ = "%";   
  magcalfit_metadata->description_ = "Goodness-of-fit of readings using current Magnetic Calibration";
  magcalfit_metadata->display_name_ = "Magnetic Calibration Fit";
  magcalfit_metadata->short_name_ = "MagCalFit";
  auto magcalfit_output = std::make_shared<SKOutput<float>>(
    kSKPathMagFit,    // Signal K path
    kConfigPathNone,  // configuration path
    magcalfit_metadata
  );
  magcalfit->connect_to(magcalfit_output);
 
  // Create output for Magnetic Calibration Fit Trial     
  auto* sensor_magcal_candidate = new OrientationValues(
      orientation_sensor, OrientationValues::kMagCalFitTrial);
  auto magcaltrial = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_magcal_candidate]() { return sensor_magcal_candidate->ReportValue(); });
  auto magcaltrial_metadata = std::make_shared<SKMetadata>();
  magcaltrial_metadata->units_ = "%";
  magcaltrial_metadata->description_ = "Goodness-of-fit of readings using trial Magnetic Calibration";
  magcaltrial_metadata->display_name_ = "Magnetic Calibration Fit Trial";
  magcaltrial_metadata->short_name_ = "MagCalFitTrial";
  auto magcaltrial_output = std::make_shared<SKOutput<float>>(
    kSKPathMagFitTrial,   // Signal K path
    kConfigPathNone,  // configuration path
    magcaltrial_metadata
  );
  magcaltrial->connect_to(magcaltrial_output);
 
  // Create output for kSKPathMagSolver
  auto* sensor_cal_order = new OrientationValues(
      orientation_sensor, OrientationValues::kMagCalAlgorithmSolver);
  auto magcalorder = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_cal_order]() { return sensor_cal_order->ReportValue(); });
  auto cal_solver_metadata = std::make_shared<SKMetadata>();
  cal_solver_metadata->units_ = "[0,4,7,10]";
  cal_solver_metadata->description_ = "Order of calibration algorithm used [0,4,7,10] 10 is best.";
  cal_solver_metadata->display_name_ = "Magnetic Calibration Algorithm Order";
  cal_solver_metadata->short_name_ = "MagCalOrder";
  auto cal_solver_output = std::make_shared<SKOutput<int>>(
    kSKPathMagSolver,  // Signal K path
    "",               // configuration path
    cal_solver_metadata
  );
  magcalorder->connect_to(cal_solver_output);
 
  // Create output for Magnetic Inclination
  auto* sensor_mag_inclination = new OrientationValues(
       orientation_sensor, OrientationValues::kMagInclination);
  auto maginclination = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_mag_inclination]() { return sensor_mag_inclination->ReportValue(); });
  auto inclination_metadata = std::make_shared<SKMetadata>();
  inclination_metadata->units_ = "rad";
  inclination_metadata->description_ = "Magnetic field inclination from horizontal";
  inclination_metadata->display_name_ = "Magnetic Inclination";
  inclination_metadata->short_name_ = "MagInclination";
  auto inclination_output = std::make_shared<SKOutput<float>>(
    kSKPathMagInclination, // Signal K path
    "",                   // configuration path
    inclination_metadata
  );
  maginclination->connect_to(inclination_output);
 
  // Create output for Magnetic B Field Strength
  auto* sensor_mag_b_value = new OrientationValues(
      orientation_sensor, OrientationValues::kMagFieldMagnitude);
  auto magbvalue = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_mag_b_value]() { return sensor_mag_b_value->ReportValue(); });
  auto magbvalue_metadata = std::make_shared<SKMetadata>();
  magbvalue_metadata->units_ = "uT";
  magbvalue_metadata->description_ = "Magnetic field strength using current calibration";
  magbvalue_metadata->display_name_ = "Magnetic B Field";
  magbvalue_metadata->short_name_ = "B Field";
  auto magbvalue_output = std::make_shared<SKOutput<float>>(
    kSKPathMagBValue, // Signal K path
    "",               // configuration path
    magbvalue_metadata
  );
  magbvalue->connect_to(magbvalue_output);
 
  // Create output for Magnetic B Field Strength using Trial Calibration
  auto* sensor_mag_b_trial_value = new OrientationValues(
      orientation_sensor, OrientationValues::kMagFieldMagnitudeTrial);
  auto magbtrialvalue = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_mag_b_trial_value]() { return sensor_mag_b_trial_value->ReportValue(); });
  auto magbtrialvalue_metadata = std::make_shared<SKMetadata>();
  magbtrialvalue_metadata->units_ = "uT";
  magbtrialvalue_metadata->description_ = "Magnetic field strength using trial calibration";
  magbtrialvalue_metadata->display_name_ = "Magnetic B Field Trial";
  magbtrialvalue_metadata->short_name_ = "B Field Trial";
  auto magbtrialvalue_output = std::make_shared<SKOutput<float>>(
    kSKPathMagBValueTrial,  // Signal K path
    "",                     // configuration path
    magbtrialvalue_metadata
  );
  magbtrialvalue->connect_to(magbtrialvalue_output);
 
  // Create output for Magnetic Noise
  auto* sensor_mag_noise = new OrientationValues(
       orientation_sensor, OrientationValues::kMagNoiseCovariance);
  auto magnoise = std::make_shared<RepeatSensor<float>>(
      4000, //update output value every 4000ms
      [sensor_mag_noise]() { return sensor_mag_noise->ReportValue(); });
  auto mag_noise_metadata = std::make_shared<SKMetadata>();
  mag_noise_metadata->units_ = "unitless";
  mag_noise_metadata->description_ = "Magnetic Noise / Interference";
  mag_noise_metadata->display_name_ = "Magnetic Noise";
  mag_noise_metadata->short_name_ = "Mag Noise"; 
  auto mag_noise_output = std::make_shared<SKOutput<float>>(
      kSKPathMagNoise,   // Signal K path
      "",  // configuration path
      mag_noise_metadata
  );
  magnoise->connect_to(mag_noise_output);
 
  // Create output for Roll Rate
  auto* sensor_roll_rate = new OrientationValues(
       orientation_sensor, OrientationValues::kRateOfRoll);
  auto roll_rate = std::make_shared<RepeatSensor<float>>(
      200, //update output value every 200ms
      [sensor_roll_rate]() { return sensor_roll_rate->ReportValue(); });
  auto metadata_roll_rate = std::make_shared<SKMetadata>();
  metadata_roll_rate->units_         = "rad/s";
  metadata_roll_rate->description_   = "Rate of Roll about bow-stern axis";
  metadata_roll_rate->display_name_  = "Roll Rate";
  metadata_roll_rate->short_name_    = "Roll Rate"; 
  auto roll_rate_output = std::make_shared<SKOutput<float>>(
      kSKPathRollRate,  // Signal K path
      "",               // configuration path
      metadata_roll_rate
  );
  roll_rate->connect_to(roll_rate_output);
  
 
  // Create output for Pitch Rate
  auto* sensor_pitch_rate = new OrientationValues(
       orientation_sensor, OrientationValues::kRateOfPitch);
  auto pitch_rate = std::make_shared<RepeatSensor<float>>(
      200, //update output value every 200ms
      [sensor_pitch_rate]() { return sensor_pitch_rate->ReportValue(); });
  auto metadata_pitch_rate = std::make_shared<SKMetadata>();
  metadata_pitch_rate->units_         = "rad/s";
  metadata_pitch_rate->description_   = "Rate of Pitch about port-starboard axis";
  metadata_pitch_rate->display_name_  = "Pitch Rate";
  metadata_pitch_rate->short_name_    = "Pitch Rate"; 
  auto pitch_rate_output = std::make_shared<SKOutput<float>>(
      kSKPathPitchRate,  // Signal K path
      "",               // configuration path
      metadata_pitch_rate
  );
  pitch_rate->connect_to(pitch_rate_output);
  
  // Create output for Turn Rate
  auto* sensor_turn_rate = new OrientationValues(
       orientation_sensor, OrientationValues::kRateOfTurn);
  auto turn_rate = std::make_shared<RepeatSensor<float>>(
      200, //update output value every 200ms
      [sensor_turn_rate]() { return sensor_turn_rate->ReportValue(); });
  auto metadata_turn_rate = std::make_shared<SKMetadata>();
  metadata_turn_rate->units_         = "rad/s";
  metadata_turn_rate->description_   = "Rate of Turn about mast-keel axis";
  metadata_turn_rate->display_name_  = "Turn Rate";
  metadata_turn_rate->short_name_    = "Turn Rate"; 
  auto turn_rate_output = std::make_shared<SKOutput<float>>(
      kSKPathTurnRate,  // Signal K path
      "",               // configuration path
      metadata_turn_rate
  );
  turn_rate->connect_to(turn_rate_output);
  
 
  // Create output for X Acceleration
  auto* sensor_accel_X = new OrientationValues(
       orientation_sensor, OrientationValues::kAccelerationX);
  auto accel_X = std::make_shared<RepeatSensor<float>>(
      1000, //update output value every 1000ms
      [sensor_accel_X]() { return sensor_accel_X->ReportValue(); });
  auto metadata_accel_X = std::make_shared<SKMetadata>();
  metadata_accel_X->units_         = "m/s^2";
  metadata_accel_X->description_   = "Acceleration in X axis of eCompass";
  metadata_accel_X->display_name_  = "X Axis Acceleration";
  metadata_accel_X->short_name_    = "Accel X"; 
  auto accel_X_output = std::make_shared<SKOutput<float>>(
      kSKPathAccelX,    // Signal K path
      "",               // configuration path
      metadata_accel_X
  );
  accel_X->connect_to(accel_X_output);
 
 
  auto* sensor_temperature = new OrientationValues(
      orientation_sensor, OrientationValues::kTemperature);
 
  auto temperature = std::make_shared<RepeatSensor<float>>(
      1000, //update output value every 1000ms
      [sensor_temperature]() { return sensor_temperature->ReportValue(); });
 
  /* Temperature readings are passed through a linear transform
  * to allow for calibration/linearization via web interface. Other
  * transforms are available. Ensure you #include the appropriate file(s).
  */
  const char* kConfigPathTemperatureCal = "/sensors/temp/calibrate";
  auto temperatureCal = std::make_shared<Linear>(1.0, 0.0,kConfigPathTemperatureCal);
  ConfigItem(temperatureCal)
      ->set_title("Compass Temperature Calibration")
      ->set_description("Calibration / Linearization of temperature reported by eCompass IC")
      ->set_sort_order(206);
 
  // Create output, with metadata to indicate source and location of temp data
  auto temperature_metadata = std::make_shared<SKMetadata>();
  temperature_metadata->units_ = "K";   
  temperature_metadata->description_ = "Temperature reported by orientation sensor";
  temperature_metadata->display_name_ = "eCompass Temperature";
  temperature_metadata->short_name_ = "Comp. T";
 
  auto temperature_output = std::make_shared<SKOutput<float>>(
      kSKPathTemperature,   // Signal K path
      kConfigPathNone,      // configuration path
      temperature_metadata
  );
 
  temperature
    ->connect_to(temperatureCal)
    ->connect_to(temperature_output);
 
  auto* button_watcher = new DigitalInputChange(
      PIN_SWITCH_CAL_SAVE, INPUT_PULLUP, CHANGE, kConfigPathNone);
  // Create a debounce transform
  const int kDebounceDelay = 350; // only react to pushes >350 ms
  const char* kConfigPathDebounceSwitch = "/debounce/delay";
  auto* debounce = new DebounceInt(kDebounceDelay, kConfigPathDebounceSwitch);
  ConfigItem(debounce)
      ->set_title("MagCal Button Debounce")
      ->set_description("Debounce delay (ms) for Magnetic Calibration save button.")
      ->set_sort_order(1000);
  // Define the action taken when button is active and debounce has elapsed.
  // Provide it with the context of orientation_sensor so it can access save fcn.
  auto save_mcal_function = [orientation_sensor](int input) {
    if (input == SWITCH_ACTIVE_STATE) {
      orientation_sensor->sensor_interface_->SaveMagneticCalibration();
      debugI("Magnetic Calibration values saved");
    }
  };
  auto* button_consumer = new LambdaConsumer<int>(save_mcal_function);
  // Connect the button -> debounce -> save magnetic calibration fcn
  button_watcher->connect_to(debounce)->connect_to(button_consumer); 
 
  
  // To avoid garbage collecting all shared pointers created in setup(),
  // loop from here.
  while (true) {
    loop();
  }
}
 
void loop() { event_loop()->tick(); }