#include <stdint.h>

#define MSP_OSD_CONFIG            84        //out message         Get osd settings - betaflight
#define MSP_NAME                  10
#define MSP_BATTERY_STATE         130       //out message         Connected/Disconnected, Voltage, Current Used

struct msp_osd_config_t {
    uint8_t osdflags;
    uint8_t video_system;
    uint8_t units;
    uint8_t rssi_alarm;
    uint16_t cap_alarm;
    uint8_t old_timer_alarm;
    uint8_t osd_item_count;                     //56
    uint16_t alt_alarm;
    uint16_t osd_rssi_value_pos;
    uint16_t osd_main_batt_voltage_pos;
    uint16_t osd_crosshairs_pos;
    uint16_t osd_artificial_horizon_pos;
    uint16_t osd_horizon_sidebars_pos;
    uint16_t osd_item_timer_1_pos;
    uint16_t osd_item_timer_2_pos;
    uint16_t osd_flymode_pos;
    uint16_t osd_craft_name_pos;
    uint16_t osd_throttle_pos_pos;
    uint16_t osd_vtx_channel_pos;
    uint16_t osd_current_draw_pos;
    uint16_t osd_mah_drawn_pos;
    uint16_t osd_gps_speed_pos;
    uint16_t osd_gps_sats_pos;
    uint16_t osd_altitude_pos;
    uint16_t osd_roll_pids_pos;
    uint16_t osd_pitch_pids_pos;
    uint16_t osd_yaw_pids_pos;
    uint16_t osd_power_pos;
    uint16_t osd_pidrate_profile_pos;
    uint16_t osd_warnings_pos;
    uint16_t osd_avg_cell_voltage_pos;
    uint16_t osd_gps_lon_pos;
    uint16_t osd_gps_lat_pos;
    uint16_t osd_debug_pos;
    uint16_t osd_pitch_angle_pos;
    uint16_t osd_roll_angle_pos;
    uint16_t osd_main_batt_usage_pos;
    uint16_t osd_disarmed_pos;
    uint16_t osd_home_dir_pos;
    uint16_t osd_home_dist_pos;
    uint16_t osd_numerical_heading_pos;
    uint16_t osd_numerical_vario_pos;
    uint16_t osd_compass_bar_pos;
    uint16_t osd_esc_tmp_pos;
    uint16_t osd_esc_rpm_pos;
    uint16_t osd_remaining_time_estimate_pos;
    uint16_t osd_rtc_datetime_pos;
    uint16_t osd_adjustment_range_pos;
    uint16_t osd_core_temperature_pos;
    uint16_t osd_anti_gravity_pos;
    uint16_t osd_g_force_pos;
    uint16_t osd_motor_diag_pos;
    uint16_t osd_log_status_pos;
    uint16_t osd_flip_arrow_pos;
    uint16_t osd_link_quality_pos;
    uint16_t osd_flight_dist_pos;
    uint16_t osd_stick_overlay_left_pos;
    uint16_t osd_stick_overlay_right_pos;
    uint16_t osd_display_name_pos;
    uint16_t osd_esc_rpm_freq_pos;
    uint16_t osd_rate_profile_name_pos;
    uint16_t osd_pid_profile_name_pos;
    uint16_t osd_profile_name_pos;
    uint16_t osd_rssi_dbm_value_pos;
    uint16_t osd_rc_channels_pos;
    uint8_t osd_stat_count;                     //24
    uint8_t osd_stat_rtc_date_time;
    uint8_t osd_stat_timer_1;
    uint8_t osd_stat_timer_2;
    uint8_t osd_stat_max_speed;
    uint8_t osd_stat_max_distance;
    uint8_t osd_stat_min_battery;
    uint8_t osd_stat_end_battery;
    uint8_t osd_stat_battery;
    uint8_t osd_stat_min_rssi;
    uint8_t osd_stat_max_current;
    uint8_t osd_stat_used_mah;
    uint8_t osd_stat_max_altitude;
    uint8_t osd_stat_blackbox;
    uint8_t osd_stat_blackbox_number;
    uint8_t osd_stat_max_g_force;
    uint8_t osd_stat_max_esc_temp;
    uint8_t osd_stat_max_esc_rpm;
    uint8_t osd_stat_min_link_quality;
    uint8_t osd_stat_flight_distance;
    uint8_t osd_stat_max_fft;
    uint8_t osd_stat_total_flights;
    uint8_t osd_stat_total_time;
    uint8_t osd_stat_total_dist;
    uint8_t osd_stat_min_rssi_dbm;
    uint16_t osd_timer_count;
    uint16_t osd_timer_1;
    uint16_t osd_timer_2;
    uint16_t enabledwarnings;
    uint8_t osd_warning_count;              // 16
    uint32_t enabledwarnings_1_41_plus;
    uint8_t osd_profile_count;              // 1
    uint8_t osdprofileindex;                // 1
    uint8_t overlay_radio_mode;             //  0
} __attribute__ ((packed));

struct msp_name_t {
    char craft_name[15];                    //15 characters max possible displayed in the goggles
} __attribute__ ((packed));

struct msp_battery_state_t {
  uint8_t batteryCellCount;
    uint16_t batteryCapacity;
    uint8_t legacyBatteryVoltage;
    uint16_t mAhDrawn;
    uint16_t amperage;
    uint8_t batteryState;
    uint16_t batteryVoltage;
} __attribute__ ((packed));

// MSP_STATUS_DJI reply
struct msp_status_DJI_t {
  uint16_t cycleTime;
  uint16_t i2cErrorCounter;
  uint16_t sensor;                    // MSP_STATUS_SENSOR_...
  uint32_t flightModeFlags;           // see getActiveModes()
  uint8_t  configProfileIndex;
  uint16_t averageSystemLoadPercent;  // 0...100
  uint16_t armingFlags;   //0x0103 or 0x0301
  uint8_t  accCalibrationAxisFlags;  //0
  uint8_t  DJI_ARMING_DISABLE_FLAGS_COUNT; //25
  uint32_t djiPackArmingDisabledFlags; //(1 << 24)
} __attribute__ ((packed));
















/*
  MSP.h

  Copyright (c) 2017, Fabrizio Di Vittorio ([email protected])

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


#pragma once

#include <Arduino.h>
#include <Stream.h>

// requests & replies 
#define MSP_API_VERSION            1
#define MSP_FC_VARIANT             2
#define MSP_FC_VERSION             3
#define MSP_BOARD_INFO             4
#define MSP_BUILD_INFO             5
#define MSP_CALIBRATION_DATA      14
#define MSP_FEATURE               36
#define MSP_BOARD_ALIGNMENT       38
#define MSP_CURRENT_METER_CONFIG  40
#define MSP_RX_CONFIG             44
#define MSP_SONAR_ALTITUDE        58
#define MSP_ARMING_CONFIG         61
#define MSP_RX_MAP                64 // get channel map (also returns number of channels total)
#define MSP_LOOP_TIME             73 // FC cycle time i.e looptime parameter
#define MSP_STATUS               101
#define MSP_RAW_IMU              102
#define MSP_SERVO                103
#define MSP_MOTOR                104
#define MSP_RC                   105
#define MSP_RAW_GPS              106
#define MSP_COMP_GPS             107 // distance home, direction home
#define MSP_ATTITUDE             108
#define MSP_ALTITUDE             109
#define MSP_ANALOG               110
#define MSP_RC_TUNING            111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID                  112 // P I D coeff
#define MSP_MISC                 114
#define MSP_SERVO_CONFIGURATIONS 120
#define MSP_NAV_STATUS           121 // navigation status
#define MSP_SENSOR_ALIGNMENT     126 // orientation of acc,gyro,mag
#define MSP_STATUS_EX            150
#define MSP_SENSOR_STATUS        151
#define MSP_BOXIDS               119
#define MSP_UID                  160 // Unique device ID
#define MSP_GPSSVINFO            164 // get Signal Strength (only U-Blox)
#define MSP_GPSSTATISTICS        166 // get GPS debugging data
#define MSP_SET_PID              202 // set P I D coeff


// commands
#define MSP_SET_HEAD             211 // define a new heading hold direction
#define MSP_SET_RAW_RC           200 // 8 rc chan
#define MSP_SET_RAW_GPS          201 // fix, numsat, lat, lon, alt, speed
#define MSP_SET_WP               209 // sets a given WP (WP#, lat, lon, alt, flags)


// bits of getActiveModes() return value 
#define MSP_MODE_ARM          0
#define MSP_MODE_ANGLE        1
#define MSP_MODE_HORIZON      2
#define MSP_MODE_NAVALTHOLD   3 /* cleanflight BARO */
#define MSP_MODE_MAG          4
#define MSP_MODE_HEADFREE     5
#define MSP_MODE_HEADADJ      6
#define MSP_MODE_CAMSTAB      7
#define MSP_MODE_NAVRTH       8 /* cleanflight GPSHOME */
#define MSP_MODE_NAVPOSHOLD   9 /* cleanflight GPSHOLD */
#define MSP_MODE_PASSTHRU    10
#define MSP_MODE_BEEPERON    11
#define MSP_MODE_LEDLOW      12
#define MSP_MODE_LLIGHTS     13
#define MSP_MODE_OSD         14
#define MSP_MODE_TELEMETRY   15
#define MSP_MODE_GTUNE       16
#define MSP_MODE_SONAR       17
#define MSP_MODE_BLACKBOX    18
#define MSP_MODE_FAILSAFE    19
#define MSP_MODE_NAVWP       20 /* cleanflight AIRMODE */
#define MSP_MODE_AIRMODE     21 /* cleanflight DISABLE3DSWITCH */
#define MSP_MODE_HOMERESET   22 /* cleanflight FPVANGLEMIX */
#define MSP_MODE_GCSNAV      23 /* cleanflight BLACKBOXERASE */
#define MSP_MODE_HEADINGLOCK 24
#define MSP_MODE_SURFACE     25
#define MSP_MODE_FLAPERON    26
#define MSP_MODE_TURNASSIST  27
#define MSP_MODE_NAVLAUNCH   28
#define MSP_MODE_AUTOTRIM    29


// MSP_API_VERSION reply
struct msp_api_version_t {
  uint8_t protocolVersion;
  uint8_t APIMajor;
  uint8_t APIMinor;
} __attribute__ ((packed));


// MSP_FC_VARIANT reply
struct msp_fc_variant_t {
  char flightControlIdentifier[4];
} __attribute__ ((packed));


// MSP_FC_VERSION reply
struct msp_fc_version_t {
  uint8_t versionMajor;
  uint8_t versionMinor;
  uint8_t versionPatchLevel;
} __attribute__ ((packed));


// MSP_BOARD_INFO reply
struct msp_board_info_t {
  char     boardIdentifier[4];
  uint16_t hardwareRevision;
} __attribute__ ((packed));


// MSP_BUILD_INFO reply
struct msp_build_info_t {
  char buildDate[11];
  char buildTime[8];
  char shortGitRevision[7];
} __attribute__ ((packed));


// MSP_RAW_IMU reply
struct msp_raw_imu_t {
  int16_t acc[3];  // x, y, z
  int16_t gyro[3]; // x, y, z
  int16_t mag[3];  // x, y, z  
} __attribute__ ((packed));


// flags for msp_status_ex_t.sensor and msp_status_t.sensor
#define MSP_STATUS_SENSOR_ACC    1
#define MSP_STATUS_SENSOR_BARO   2
#define MSP_STATUS_SENSOR_MAG    4
#define MSP_STATUS_SENSOR_GPS    8
#define MSP_STATUS_SENSOR_SONAR 16


// MSP_STATUS_EX reply
struct msp_status_ex_t {
  uint16_t cycleTime;
  uint16_t i2cErrorCounter;
  uint16_t sensor;                    // MSP_STATUS_SENSOR_...
  uint32_t flightModeFlags;           // see getActiveModes()
  uint8_t  configProfileIndex;
  uint16_t averageSystemLoadPercent;  // 0...100
  uint16_t armingFlags;
  uint8_t  accCalibrationAxisFlags;
} __attribute__ ((packed));


// MSP_STATUS
struct msp_status_t {
  uint16_t cycleTime;
  uint16_t i2cErrorCounter;
  uint16_t sensor;                    // MSP_STATUS_SENSOR_...
  uint32_t flightModeFlags;           // see getActiveModes()
  uint8_t  configProfileIndex;
} __attribute__ ((packed));


// MSP_SENSOR_STATUS reply
struct msp_sensor_status_t {
  uint8_t isHardwareHealthy;  // 0...1
  uint8_t hwGyroStatus;
  uint8_t hwAccelerometerStatus;
  uint8_t hwCompassStatus;
  uint8_t hwBarometerStatus;
  uint8_t hwGPSStatus;
  uint8_t hwRangefinderStatus;
  uint8_t hwPitotmeterStatus;
  uint8_t hwOpticalFlowStatus;  
} __attribute__ ((packed));


#define MSP_MAX_SUPPORTED_SERVOS 8

// MSP_SERVO reply
struct msp_servo_t {
  uint16_t servo[MSP_MAX_SUPPORTED_SERVOS];
} __attribute__ ((packed));


// MSP_SERVO_CONFIGURATIONS reply
struct msp_servo_configurations_t {
  __attribute__ ((packed)) struct {
    uint16_t min;
    uint16_t max;
    uint16_t middle;
    uint8_t rate;
    uint8_t angleAtMin;
    uint8_t angleAtMax;
    uint8_t forwardFromChannel;
    uint32_t reversedSources;
  } conf[MSP_MAX_SUPPORTED_SERVOS];
} __attribute__ ((packed));


#define MSP_MAX_SERVO_RULES (2 * MSP_MAX_SUPPORTED_SERVOS)

// MSP_SERVO_MIX_RULES reply
struct msp_servo_mix_rules_t {
  __attribute__ ((packed)) struct {
    uint8_t targetChannel;
    uint8_t inputSource;
    uint8_t rate;
    uint8_t speed;
    uint8_t min;
    uint8_t max;
  } mixRule[MSP_MAX_SERVO_RULES];
} __attribute__ ((packed));


#define MSP_MAX_SUPPORTED_MOTORS 8

// MSP_MOTOR reply
struct msp_motor_t {
  uint16_t motor[MSP_MAX_SUPPORTED_MOTORS];
} __attribute__ ((packed));


#define MSP_MAX_SUPPORTED_CHANNELS 16

// MSP_RC reply
struct msp_rc_t {
  uint16_t channelValue[MSP_MAX_SUPPORTED_CHANNELS];
} __attribute__ ((packed));


// MSP_ATTITUDE reply
struct msp_attitude_t {
  int16_t roll;
  int16_t pitch;
  int16_t yaw;
} __attribute__ ((packed));


// MSP_ALTITUDE reply
struct msp_altitude_t {
  int32_t estimatedActualPosition;  // cm
  int16_t estimatedActualVelocity;  // cm/s
  int32_t baroLatestAltitude;      
} __attribute__ ((packed));


// MSP_SONAR_ALTITUDE reply
struct msp_sonar_altitude_t {
  int32_t altitude;
} __attribute__ ((packed));


// MSP_ANALOG reply
struct msp_analog_t {
  uint8_t  vbat;     // 0...255
  uint16_t mAhDrawn; // milliamp hours drawn from battery
  uint16_t rssi;     // 0..1023
  int16_t  amperage; // send amperage in 0.01 A steps, range is -320A to 320A  
} __attribute__ ((packed));


// MSP_ARMING_CONFIG reply
struct msp_arming_config_t {
  uint8_t auto_disarm_delay;
  uint8_t disarm_kill_switch;
} __attribute__ ((packed));


// MSP_LOOP_TIME reply
struct msp_loop_time_t {
  uint16_t looptime;
} __attribute__ ((packed));


// MSP_RC_TUNING reply
struct msp_rc_tuning_t {
  uint8_t  rcRate8;  // no longer used
  uint8_t  rcExpo8;
  uint8_t  rates[3]; // R,P,Y
  uint8_t  dynThrPID;
  uint8_t  thrMid8;
  uint8_t  thrExpo8;
  uint16_t tpa_breakpoint;
  uint8_t  rcYawExpo8;  
} __attribute__ ((packed));


// MSP_PID reply
struct msp_pid_t {
  uint8_t roll[3];     // 0=P, 1=I, 2=D
  uint8_t pitch[3];    // 0=P, 1=I, 2=D
  uint8_t yaw[3];      // 0=P, 1=I, 2=D
  uint8_t pos_z[3];    // 0=P, 1=I, 2=D
  uint8_t pos_xy[3];   // 0=P, 1=I, 2=D
  uint8_t vel_xy[3];   // 0=P, 1=I, 2=D
  uint8_t surface[3];  // 0=P, 1=I, 2=D
  uint8_t level[3];    // 0=P, 1=I, 2=D
  uint8_t heading[3];  // 0=P, 1=I, 2=D
  uint8_t vel_z[3];    // 0=P, 1=I, 2=D
} __attribute__ ((packed));


// MSP_MISC reply
struct msp_misc_t {
  uint16_t midrc;
  uint16_t minthrottle;
  uint16_t maxthrottle;
  uint16_t mincommand;
  uint16_t failsafe_throttle;
  uint8_t  gps_provider;
  uint8_t  gps_baudrate;
  uint8_t  gps_ubx_sbas;
  uint8_t  multiwiiCurrentMeterOutput;
  uint8_t  rssi_channel;
  uint8_t  dummy;
  uint16_t mag_declination;
  uint8_t  vbatscale;
  uint8_t  vbatmincellvoltage;
  uint8_t  vbatmaxcellvoltage;
  uint8_t  vbatwarningcellvoltage;
} __attribute__ ((packed));


// values for msp_raw_gps_t.fixType
#define MSP_GPS_NO_FIX 0
#define MSP_GPS_FIX_2D 1
#define MSP_GPS_FIX_3D 2


// MSP_RAW_GPS reply
struct msp_raw_gps_t {
  uint8_t  fixType;       // MSP_GPS_NO_FIX, MSP_GPS_FIX_2D, MSP_GPS_FIX_3D
  uint8_t  numSat;
  int32_t  lat;           // 1 / 10000000 deg
  int32_t  lon;           // 1 / 10000000 deg
  int16_t  alt;           // meters
  int16_t  groundSpeed;   // cm/s
  int16_t  groundCourse;  // unit: degree x 10
  uint16_t hdop;
} __attribute__ ((packed));


// MSP_COMP_GPS reply
struct msp_comp_gps_t {
  int16_t  distanceToHome;  // distance to home in meters
  int16_t  directionToHome; // direction to home in degrees
  uint8_t  heartbeat;       // toggles 0 and 1 for each change
} __attribute__ ((packed));


// values for msp_nav_status_t.mode
#define MSP_NAV_STATUS_MODE_NONE   0
#define MSP_NAV_STATUS_MODE_HOLD   1
#define MSP_NAV_STATUS_MODE_RTH    2
#define MSP_NAV_STATUS_MODE_NAV    3
#define MSP_NAV_STATUS_MODE_EMERG 15

// values for msp_nav_status_t.state
#define MSP_NAV_STATUS_STATE_NONE                0  // None
#define MSP_NAV_STATUS_STATE_RTH_START           1  // RTH Start
#define MSP_NAV_STATUS_STATE_RTH_ENROUTE         2  // RTH Enroute
#define MSP_NAV_STATUS_STATE_HOLD_INFINIT        3  // PosHold infinit
#define MSP_NAV_STATUS_STATE_HOLD_TIMED          4  // PosHold timed
#define MSP_NAV_STATUS_STATE_WP_ENROUTE          5  // WP Enroute
#define MSP_NAV_STATUS_STATE_PROCESS_NEXT        6  // Process next
#define MSP_NAV_STATUS_STATE_DO_JUMP             7  // Jump
#define MSP_NAV_STATUS_STATE_LAND_START          8  // Start Land
#define MSP_NAV_STATUS_STATE_LAND_IN_PROGRESS    9  // Land in Progress
#define MSP_NAV_STATUS_STATE_LANDED             10  // Landed
#define MSP_NAV_STATUS_STATE_LAND_SETTLE        11  // Settling before land
#define MSP_NAV_STATUS_STATE_LAND_START_DESCENT 12  // Start descent

// values for msp_nav_status_t.activeWpAction, msp_set_wp_t.action
#define MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT 0x01
#define MSP_NAV_STATUS_WAYPOINT_ACTION_RTH      0x04

// values for msp_nav_status_t.error
#define MSP_NAV_STATUS_ERROR_NONE               0   // All systems clear
#define MSP_NAV_STATUS_ERROR_TOOFAR             1   // Next waypoint distance is more than safety distance
#define MSP_NAV_STATUS_ERROR_SPOILED_GPS        2   // GPS reception is compromised - Nav paused - copter is adrift !
#define MSP_NAV_STATUS_ERROR_WP_CRC             3   // CRC error reading WP data from EEPROM - Nav stopped
#define MSP_NAV_STATUS_ERROR_FINISH             4   // End flag detected, navigation finished
#define MSP_NAV_STATUS_ERROR_TIMEWAIT           5   // Waiting for poshold timer
#define MSP_NAV_STATUS_ERROR_INVALID_JUMP       6   // Invalid jump target detected, aborting
#define MSP_NAV_STATUS_ERROR_INVALID_DATA       7   // Invalid mission step action code, aborting, copter is adrift
#define MSP_NAV_STATUS_ERROR_WAIT_FOR_RTH_ALT   8   // Waiting to reach RTH Altitude
#define MSP_NAV_STATUS_ERROR_GPS_FIX_LOST       9   // Gps fix lost, aborting mission
#define MSP_NAV_STATUS_ERROR_DISARMED          10   // NAV engine disabled due disarm
#define MSP_NAV_STATUS_ERROR_LANDING           11   // Landing


// MSP_NAV_STATUS reply
struct msp_nav_status_t {
  uint8_t mode;           // one of MSP_NAV_STATUS_MODE_XXX
  uint8_t state;          // one of MSP_NAV_STATUS_STATE_XXX
  uint8_t activeWpAction; // combination of MSP_NAV_STATUS_WAYPOINT_ACTION_XXX
  uint8_t activeWpNumber;
  uint8_t error;          // one of MSP_NAV_STATUS_ERROR_XXX
  int16_t magHoldHeading;
} __attribute__ ((packed));


// MSP_GPSSVINFO reply
struct msp_gpssvinfo_t {
  uint8_t dummy1;
  uint8_t dummy2;
  uint8_t dummy3;
  uint8_t dummy4;
  uint8_t HDOP;
} __attribute__ ((packed));


// MSP_GPSSTATISTICS reply
struct msp_gpsstatistics_t {
  uint16_t lastMessageDt;
  uint32_t errors;
  uint32_t timeouts;
  uint32_t packetCount;
  uint16_t hdop;
  uint16_t eph;
  uint16_t epv;
} __attribute__ ((packed));


// MSP_UID reply
struct msp_uid_t {
  uint32_t uid0;
  uint32_t uid1;
  uint32_t uid2;
} __attribute__ ((packed));


// MSP_FEATURE mask
#define MSP_FEATURE_RX_PPM              (1 <<  0)
#define MSP_FEATURE_VBAT                (1 <<  1)
#define MSP_FEATURE_UNUSED_1            (1 <<  2)
#define MSP_FEATURE_RX_SERIAL           (1 <<  3)
#define MSP_FEATURE_MOTOR_STOP          (1 <<  4)
#define MSP_FEATURE_SERVO_TILT          (1 <<  5)
#define MSP_FEATURE_SOFTSERIAL          (1 <<  6)
#define MSP_FEATURE_GPS                 (1 <<  7)
#define MSP_FEATURE_UNUSED_3            (1 <<  8)         // was FEATURE_FAILSAFE
#define MSP_FEATURE_UNUSED_4            (1 <<  9)         // was FEATURE_SONAR
#define MSP_FEATURE_TELEMETRY           (1 << 10)
#define MSP_FEATURE_CURRENT_METER       (1 << 11)
#define MSP_FEATURE_3D                  (1 << 12)
#define MSP_FEATURE_RX_PARALLEL_PWM     (1 << 13)
#define MSP_FEATURE_RX_MSP              (1 << 14)
#define MSP_FEATURE_RSSI_ADC            (1 << 15)
#define MSP_FEATURE_LED_STRIP           (1 << 16)
#define MSP_FEATURE_DASHBOARD           (1 << 17)
#define MSP_FEATURE_UNUSED_2            (1 << 18)
#define MSP_FEATURE_BLACKBOX            (1 << 19)
#define MSP_FEATURE_CHANNEL_FORWARDING  (1 << 20)
#define MSP_FEATURE_TRANSPONDER         (1 << 21)
#define MSP_FEATURE_AIRMODE             (1 << 22)
#define MSP_FEATURE_SUPEREXPO_RATES     (1 << 23)
#define MSP_FEATURE_VTX                 (1 << 24)
#define MSP_FEATURE_RX_SPI              (1 << 25)
#define MSP_FEATURE_SOFTSPI             (1 << 26)
#define MSP_FEATURE_PWM_SERVO_DRIVER    (1 << 27)
#define MSP_FEATURE_PWM_OUTPUT_ENABLE   (1 << 28)
#define MSP_FEATURE_OSD                 (1 << 29)


// MSP_FEATURE reply
struct msp_feature_t {
  uint32_t featureMask; // combination of MSP_FEATURE_XXX
} __attribute__ ((packed));


// MSP_BOARD_ALIGNMENT reply
struct msp_board_alignment_t {
  int16_t rollDeciDegrees;
  int16_t pitchDeciDegrees;
  int16_t yawDeciDegrees;
} __attribute__ ((packed));


// values for msp_current_meter_config_t.currentMeterType
#define MSP_CURRENT_SENSOR_NONE    0
#define MSP_CURRENT_SENSOR_ADC     1
#define MSP_CURRENT_SENSOR_VIRTUAL 2
#define MSP_CURRENT_SENSOR_MAX     CURRENT_SENSOR_VIRTUAL
 

// MSP_CURRENT_METER_CONFIG reply
struct msp_current_meter_config_t {
  int16_t currentMeterScale;
  int16_t currentMeterOffset;
  uint8_t currentMeterType; // MSP_CURRENT_SENSOR_XXX
  uint16_t batteryCapacity;  
} __attribute__ ((packed));


// msp_rx_config_t.serialrx_provider
#define MSP_SERIALRX_SPEKTRUM1024      0
#define MSP_SERIALRX_SPEKTRUM2048      1
#define MSP_SERIALRX_SBUS              2
#define MSP_SERIALRX_SUMD              3
#define MSP_SERIALRX_SUMH              4
#define MSP_SERIALRX_XBUS_MODE_B       5
#define MSP_SERIALRX_XBUS_MODE_B_RJ01  6
#define MSP_SERIALRX_IBUS              7
#define MSP_SERIALRX_JETIEXBUS         8
#define MSP_SERIALRX_CRSF              9


// msp_rx_config_t.rx_spi_protocol values
#define MSP_SPI_PROT_NRF24RX_V202_250K 0
#define MSP_SPI_PROT_NRF24RX_V202_1M   1
#define MSP_SPI_PROT_NRF24RX_SYMA_X    2
#define MSP_SPI_PROT_NRF24RX_SYMA_X5C  3
#define MSP_SPI_PROT_NRF24RX_CX10      4
#define MSP_SPI_PROT_NRF24RX_CX10A     5
#define MSP_SPI_PROT_NRF24RX_H8_3D     6
#define MSP_SPI_PROT_NRF24RX_INAV      7


// MSP_RX_CONFIG reply
struct msp_rx_config_t {
  uint8_t   serialrx_provider;  // one of MSP_SERIALRX_XXX values
  uint16_t  maxcheck;
  uint16_t  midrc;
  uint16_t  mincheck;
  uint8_t   spektrum_sat_bind;
  uint16_t  rx_min_usec;
  uint16_t  rx_max_usec;
  uint8_t   dummy1;
  uint8_t   dummy2; 
  uint16_t  dummy3;
  uint8_t   rx_spi_protocol;  // one of MSP_SPI_PROT_XXX values
  uint32_t  rx_spi_id;
  uint8_t   rx_spi_rf_channel_count; 
} __attribute__ ((packed));


#define MSP_MAX_MAPPABLE_RX_INPUTS 8

// MSP_RX_MAP reply
struct msp_rx_map_t {
  uint8_t rxmap[MSP_MAX_MAPPABLE_RX_INPUTS];  // [0]=roll channel, [1]=pitch channel, [2]=yaw channel, [3]=throttle channel, [3+n]=aux n channel, etc...
} __attribute__ ((packed));


// values for msp_sensor_alignment_t.gyro_align, acc_align, mag_align
#define MSP_SENSOR_ALIGN_CW0_DEG        1
#define MSP_SENSOR_ALIGN_CW90_DEG       2
#define MSP_SENSOR_ALIGN_CW180_DEG      3
#define MSP_SENSOR_ALIGN_CW270_DEG      4
#define MSP_SENSOR_ALIGN_CW0_DEG_FLIP   5
#define MSP_SENSOR_ALIGN_CW90_DEG_FLIP  6
#define MSP_SENSOR_ALIGN_CW180_DEG_FLIP 7
#define MSP_SENSOR_ALIGN_CW270_DEG_FLIP 8

// MSP_SENSOR_ALIGNMENT reply
struct msp_sensor_alignment_t {
  uint8_t gyro_align;   // one of MSP_SENSOR_ALIGN_XXX
  uint8_t acc_align;    // one of MSP_SENSOR_ALIGN_XXX
  uint8_t mag_align;    // one of MSP_SENSOR_ALIGN_XXX
} __attribute__ ((packed));


// MSP_CALIBRATION_DATA reply
struct msp_calibration_data_t {
  int16_t accZeroX;
  int16_t accZeroY;
  int16_t accZeroZ;
  int16_t accGainX;
  int16_t accGainY;
  int16_t accGainZ;
  int16_t magZeroX;
  int16_t magZeroY;
  int16_t magZeroZ;
} __attribute__ ((packed));


// MSP_SET_HEAD command
struct msp_set_head_t {
  int16_t magHoldHeading; // degrees
} __attribute__ ((packed));


// MSP_SET_RAW_RC command
struct msp_set_raw_rc_t {
  uint16_t channel[MSP_MAX_SUPPORTED_CHANNELS];
} __attribute__ ((packed));


// MSP_SET_PID command
typedef msp_pid_t msp_set_pid_t;


// MSP_SET_RAW_GPS command
struct msp_set_raw_gps_t {
  uint8_t  fixType;       // MSP_GPS_NO_FIX, MSP_GPS_FIX_2D, MSP_GPS_FIX_3D
  uint8_t  numSat;
  int32_t  lat;           // 1 / 10000000 deg
  int32_t  lon;           // 1 / 10000000 deg
  int16_t  alt;           // meters
  int16_t  groundSpeed;   // cm/s
} __attribute__ ((packed));


// MSP_SET_WP command
// Special waypoints are 0 and 255. 0 is the RTH position, 255 is the POSHOLD position (lat, lon, alt).
struct msp_set_wp_t {
  uint8_t waypointNumber;  
  uint8_t action;   // one of MSP_NAV_STATUS_WAYPOINT_ACTION_XXX
  int32_t lat;      // decimal degrees latitude * 10000000
  int32_t lon;      // decimal degrees longitude * 10000000
  int32_t alt;      // altitude (cm)
  int16_t p1;       // speed (cm/s) when action is MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT, or "land" (value 1) when action is MSP_NAV_STATUS_WAYPOINT_ACTION_RTH
  int16_t p2;       // not used
  int16_t p3;       // not used
  uint8_t flag;     // 0xa5 = last, otherwise set to 0
} __attribute__ ((packed));




/////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////

class MSP {
  
  public:

    void begin(Stream & stream, uint32_t timeout = 500);

    // low level functions

    void send(uint8_t messageID, void * payload, uint8_t size);
    bool recv(uint8_t * messageID, void * payload, uint8_t maxSize, uint8_t * recvSize); 

    bool activityDetected();

    bool waitFor(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize = NULL);
    
    bool request(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize = NULL);

    bool command(uint8_t messageID, void * payload, uint8_t size, bool waitACK = true);

    void reset();

    // high level functions
    
    bool getActiveModes(uint32_t * activeModes);
    

  private:
  
    Stream * _stream;
    uint32_t _timeout;
    
};






















/*
  MSP.cpp

  Copyright (c) 2017, Fabrizio Di Vittorio ([email protected])

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include <Arduino.h>


void MSP::begin(Stream & stream, uint32_t timeout)
{
  _stream   = &stream;
  _timeout  = timeout;
}


void MSP::reset()
{
  _stream->flush();
  while (_stream->available() > 0)
    _stream->read();
}

void MSP::send(uint8_t messageID, void * payload, uint8_t size)
{
  _stream->write('$');
  _stream->write('M');
  _stream->write('<');
  _stream->write(size);
  _stream->write(messageID);
  uint8_t checksum = size ^ messageID;
  uint8_t * payloadPtr = (uint8_t*)payload;
  for (uint8_t i = 0; i < size; ++i) {
    uint8_t b = *(payloadPtr++);
    checksum ^= b;
    _stream->write(b);
  }
  _stream->write(checksum);
}


// timeout in milliseconds
bool MSP::recv(uint8_t * messageID, void * payload, uint8_t maxSize, uint8_t * recvSize)
{
  uint32_t t0 = millis();

  while (1) {
    
    // read header
    while (_stream->available() < 6)
      if (millis() - t0 >= _timeout)
        return false;
    char header[3];
    _stream->readBytes((char*)header, 3);

    // check header
    if (header[0] == '$' && header[1] == 'M' && (header[2] == '>' || header[2] == '<')) {

      // header ok, read payload size
      *recvSize = _stream->read();

      // read message ID (type)
      *messageID = _stream->read();

      uint8_t checksumCalc = *recvSize ^ *messageID;

      // read payload
      uint8_t * payloadPtr = (uint8_t*)payload;
      uint8_t idx = 0;
      while (idx < *recvSize) {
        if (millis() - t0 >= _timeout)
          return false;
        if (_stream->available() > 0) {
          uint8_t b = _stream->read();
          checksumCalc ^= b;
          if (idx < maxSize)
            *(payloadPtr++) = b;
          ++idx;
        }
      }
      // zero remaining bytes if *size < maxSize
      for (; idx < maxSize; ++idx)
        *(payloadPtr++) = 0;

      // read and check checksum
      while (_stream->available() == 0)
        if (millis() - t0 >= _timeout)
          return false;
      uint8_t checksum = _stream->read();
      if (checksumCalc == checksum) {
        return true;
      }
      
    }
  }
  
}

bool MSP::activityDetected() 
{
  uint32_t t0 = millis();

  while (1) {
    return true;
    // read header
    while (_stream->available() < 6)
      if (millis() - t0 >= _timeout)
        return false;
        
    return true; 
  }
  
}

// wait for messageID
// recvSize can be NULL
bool MSP::waitFor(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize)
{
  uint8_t recvMessageID;
  uint8_t recvSizeValue;
  uint32_t t0 = millis();
  while (millis() - t0 < _timeout)
    if (recv(&recvMessageID, payload, maxSize, (recvSize ? recvSize : &recvSizeValue)) && messageID == recvMessageID)
      return true;

  // timeout
  return false;  
}


// send a message and wait for the reply
// recvSize can be NULL
bool MSP::request(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize)
{
  send(messageID, NULL, 0);
  return waitFor(messageID, payload, maxSize, recvSize);
}


// send message and wait for ack
bool MSP::command(uint8_t messageID, void * payload, uint8_t size, bool waitACK)
{
  send(messageID, payload, size);

  // ack required
  if (waitACK)
    return waitFor(messageID, NULL, 0);
  
  return true;
}


// map MSP_MODE_xxx to box ids
// mixed values from cleanflight and inav
static const uint8_t BOXIDS[30] PROGMEM = {
  0,  //  0: MSP_MODE_ARM
  1,  //  1: MSP_MODE_ANGLE
  2,  //  2: MSP_MODE_HORIZON
  3,  //  3: MSP_MODE_NAVALTHOLD (cleanflight BARO)
  5,  //  4: MSP_MODE_MAG
  6,  //  5: MSP_MODE_HEADFREE
  7,  //  6: MSP_MODE_HEADADJ
  8,  //  7: MSP_MODE_CAMSTAB
  10, //  8: MSP_MODE_NAVRTH (cleanflight GPSHOME)
  11, //  9: MSP_MODE_NAVPOSHOLD (cleanflight GPSHOLD)
  12, // 10: MSP_MODE_PASSTHRU
  13, // 11: MSP_MODE_BEEPERON
  15, // 12: MSP_MODE_LEDLOW
  16, // 13: MSP_MODE_LLIGHTS
  19, // 14: MSP_MODE_OSD
  20, // 15: MSP_MODE_TELEMETRY
  21, // 16: MSP_MODE_GTUNE
  22, // 17: MSP_MODE_SONAR
  26, // 18: MSP_MODE_BLACKBOX
  27, // 19: MSP_MODE_FAILSAFE
  28, // 20: MSP_MODE_NAVWP (cleanflight AIRMODE)
  29, // 21: MSP_MODE_AIRMODE (cleanflight DISABLE3DSWITCH)
  30, // 22: MSP_MODE_HOMERESET (cleanflight FPVANGLEMIX)
  31, // 23: MSP_MODE_GCSNAV (cleanflight BLACKBOXERASE)
  32, // 24: MSP_MODE_HEADINGLOCK
  33, // 25: MSP_MODE_SURFACE
  34, // 26: MSP_MODE_FLAPERON
  35, // 27: MSP_MODE_TURNASSIST
  36, // 28: MSP_MODE_NAVLAUNCH
  37, // 29: MSP_MODE_AUTOTRIM
};


// returns active mode (using MSP_STATUS and MSP_BOXIDS messages)
// see MSP_MODE_... for bits inside activeModes
bool MSP::getActiveModes(uint32_t * activeModes)
{
  // request status ex
  msp_status_t status;
  if (request(MSP_STATUS, &status, sizeof(status))) {
    // request permanent ids associated to boxes
    uint8_t ids[sizeof(BOXIDS)];
    uint8_t recvSize;
    if (request(MSP_BOXIDS, ids, sizeof(ids), &recvSize)) {
      // compose activeModes, converting BOXIDS to bit map (setting 1 if related flag in flightModeFlags is set)
      *activeModes = 0;
      for (uint8_t i = 0; i < recvSize; ++i) {
        if (status.flightModeFlags & (1 << i)) {
          for (uint8_t j = 0; j < sizeof(BOXIDS); ++j) {
            if (pgm_read_byte(BOXIDS + j) == ids[i]) {
              *activeModes |= 1 << j;   
              break;
            }
          }
        }
      }  
      return true;
    }
  }

  return false;
}



//OSD elements positions
//in betaflight configurator set OSD elements to your desired positions and in CLI type "set osd" to retreieve the numbers.
//234 -> not visible. Horizontally 2048-2074(spacing 1), vertically 2048-2528(spacing 32). 26 characters X 15 lines

#define SAMD21  
//#define DEBUG    // uncomment this line to debug in Serial Monitor 

#include <stdint.h>

//currently working elements
const uint16_t osd_avg_cell_voltage_pos = 2273;
const uint16_t osd_main_batt_voltage_pos = 2241;
const uint16_t osd_crosshairs_pos = 2254;
const uint16_t osd_craft_name_pos = 2208;

const uint16_t osd_altitude_pos = 234;
uint16_t osd_gps_sats_pos = 234;
const uint16_t osd_home_dir_pos = 234;
const uint16_t osd_home_dist_pos = 234;
const uint16_t osd_gps_speed_pos = 234;
const uint16_t osd_gps_lat_pos = 234;
const uint16_t osd_gps_lon_pos = 234;

const uint16_t osd_pitch_angle_pos = 234;
const uint16_t osd_roll_angle_pos = 234;
const uint16_t osd_rssi_value_pos = 234;
const uint16_t osd_display_name_pos = 234;
const uint16_t osd_flymode_pos = 234;
const uint16_t osd_current_draw_pos = 234;
const uint16_t osd_mah_drawn_pos = 234;
const uint16_t osd_numerical_vario_pos = 234;

//not implemented or not available
const uint16_t osd_throttle_pos_pos = 234;
const uint16_t osd_vtx_channel_pos = 234;
const uint16_t osd_roll_pids_pos = 234;
const uint16_t osd_pitch_pids_pos = 234;
const uint16_t osd_yaw_pids_pos = 234;
const uint16_t osd_power_pos = 234;
const uint16_t osd_pidrate_profile_pos = 234;
const uint16_t osd_warnings_pos = 234;
const uint16_t osd_debug_pos = 234;
const uint16_t osd_artificial_horizon_pos = 234;
const uint16_t osd_horizon_sidebars_pos = 234;
const uint16_t osd_item_timer_1_pos = 234;
const uint16_t osd_item_timer_2_pos = 234;
const uint16_t osd_main_batt_usage_pos = 234;
const uint16_t osd_disarmed_pos = 234;
const uint16_t osd_numerical_heading_pos = 234;
const uint16_t osd_compass_bar_pos = 234;
const uint16_t osd_esc_tmp_pos = 234;
const uint16_t osd_esc_rpm_pos = 234;
const uint16_t osd_remaining_time_estimate_pos = 234;
const uint16_t osd_rtc_datetime_pos = 234;
const uint16_t osd_adjustment_range_pos = 234;
const uint16_t osd_core_temperature_pos = 234;
const uint16_t osd_anti_gravity_pos = 234;
const uint16_t osd_g_force_pos = 234;
const uint16_t osd_motor_diag_pos = 234;
const uint16_t osd_log_status_pos = 234;
const uint16_t osd_flip_arrow_pos = 234;
const uint16_t osd_link_quality_pos = 234;
const uint16_t osd_flight_dist_pos = 234;
const uint16_t osd_stick_overlay_left_pos = 234;
const uint16_t osd_stick_overlay_right_pos = 234;
const uint16_t osd_esc_rpm_freq_pos = 234;
const uint16_t osd_rate_profile_name_pos = 234;
const uint16_t osd_pid_profile_name_pos = 234;
const uint16_t osd_profile_name_pos = 234;
const uint16_t osd_rssi_dbm_value_pos = 234;
const uint16_t osd_rc_channels_pos = 234;
















/************************************************************************************
  @file       arduino_DJI_03_RC_ARM.ino
  @brief      Send DJI Arm using XIAO Seeeduino over MSP to keep power level at full.
  @author     Richard Amiss
  
  Release Note:  
  Complete rework for XIAO Seeeduino and MSP libraries. RC input is no longer used in 
  this version.  The AU will simply arm a few seconds after being turned on, as long 
  as the goggles are on.
  
  Code:        Richard Amiss
  Version:     1.1.0
  Date:        06/24/23

  Credits:
  This software is based on and uses software published by 2022 David Payne:
  QLiteOSD, which is based on work by Paul Kurucz (pkuruz):opentelem_to_bst_bridge 
  as well as software from d3ngit : djihdfpv_mavlink_to_msp_V2 and 
  crashsalot : VOT_to_DJIFPV

THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES, WHETHER EXPRESS, 
IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF 
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE 
COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 
CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
************************************************************************************/


#define ANALOG_IN                A0    // Voltage Read pin (notice this is now Pin 0, instead of Pin 1)
#define VOLT_DIVIDER             48    // Set to 1024/full scale voltage
#define DEBUG                          //uncomment to see diagnostics from USB serial

#define FC_FIRMWARE_NAME          "Betaflight"
#define FC_FIRMWARE_IDENTIFIER    "BTFL"

HardwareSerial &mspSerial = Serial1;
MSP msp;

// Arm Logic
uint32_t unarmedMillis = 3000;   // number of milliseconds to wait before arming, after AU is initialized. Recommend at least 3000 (3 seconds).

//Voltage and Battery Reading
float averageVoltage = 0;
int sampleVoltageCount = 0;
int lastCount = 0;

boolean lightOn = true;

uint8_t *messageID;
void *payload;
uint8_t maxSize;
uint8_t *recvSize;

//Other
char fcVariant[5] = "BTFL";
char craftname[15] = "Craft";
uint32_t previousMillis_MSP = 0;
uint32_t activityDetectedMillis_MSP = 0;
bool activityDetected = false;
const uint32_t next_interval_MSP = 100;
uint32_t general_counter = next_interval_MSP;
uint32_t custom_mode = 0;  //flight mode
uint8_t vbat = 0;
uint32_t flightModeFlags = 0x00000002;
uint8_t batteryCellCount = 3;
uint32_t previousFlightMode = custom_mode;

msp_battery_state_t battery_state = { 0 };
msp_name_t name = { 0 };
msp_fc_version_t fc_version = { 0 };
msp_fc_variant_t fc_variant = { 0 };
//msp_status_BF_t status_BF = {0};
msp_status_DJI_t status_DJI = { 0 };
msp_analog_t analog = { 0 };

void setup() {
  #ifdef DEBUG
    Serial.begin(115200);
    while (!Serial);
  #endif
  Serial1.begin(115200);
  while (!Serial1);

  analogReadResolution(12); // SAMD21 12 bit resolution 0 - 4096 range on Analog pin

  msp.begin(mspSerial);
  //pinMode(LED_BUILTIN, OUTPUT);

  delay(1000);

  status_DJI.cycleTime = 0x0080;
  status_DJI.i2cErrorCounter = 0;
  status_DJI.sensor = 0x23;
  status_DJI.configProfileIndex = 0;
  status_DJI.averageSystemLoadPercent = 7;
  status_DJI.accCalibrationAxisFlags = 0;
  status_DJI.DJI_ARMING_DISABLE_FLAGS_COUNT = 20;
  status_DJI.djiPackArmingDisabledFlags = (1 << 24);
  flightModeFlags = 0x00000002;

  #ifdef DEBUG
    Serial.println("Initialized");
  #endif
}

void loop() { 
   

  if (!activityDetected) {
    #ifdef DEBUG
      Serial.println("Waiting for AU...");
    #endif
    //digitalWrite(LED_BUILTIN, LOW);

    // Wait for Air Unit to send data
    while(!msp.activityDetected()) {    
    };
    activityDetected = true;
    activityDetectedMillis_MSP = millis();    
    #ifdef DEBUG
      Serial.println("AU Detected, waiting (" + String(unarmedMillis)+ ") time till arm");
    #endif
  }

  //digitalWrite(LED_BUILTIN, HIGH);
  
  uint32_t currentMillis_MSP = millis();

  if ((uint32_t)(currentMillis_MSP - previousMillis_MSP) >= next_interval_MSP) {
    previousMillis_MSP = currentMillis_MSP;

    if (general_counter % 300 == 0) {  // update the altitude and voltage values every 300ms
      getVoltageSample();
      if (lightOn) {
        //digitalWrite(LED_BUILTIN, LOW);
      } else {
        //digitalWrite(LED_BUILTIN, HIGH);
      }
      lightOn = !lightOn;
    }

    if (currentMillis_MSP < (activityDetectedMillis_MSP + unarmedMillis)) {
      set_flight_mode_flags(false);
    } else {
      set_flight_mode_flags(true);
    }

#ifdef DEBUG
    //debugPrint();
#endif
    send_msp_to_airunit();
    general_counter += next_interval_MSP;
  }
  if (custom_mode != previousFlightMode) {
    previousFlightMode = custom_mode;
    display_flight_mode();
  }

  if (batteryCellCount == 0 && vbat > 0) {
    set_battery_cells_number();
  }

  //display flight mode every 10s
  if (general_counter % 10000 == 0) {
    display_flight_mode();
  }
}

msp_osd_config_t msp_osd_config = { 0 };

void send_osd_config() {

#ifdef IMPERIAL_UNITS
  msp_osd_config.units = 0;
#else
  msp_osd_config.units = 1;
#endif

  msp_osd_config.osd_item_count = 56;
  msp_osd_config.osd_stat_count = 24;
  msp_osd_config.osd_timer_count = 2;
  msp_osd_config.osd_warning_count = 16;  // 16
  msp_osd_config.osd_profile_count = 1;   // 1
  msp_osd_config.osdprofileindex = 1;     // 1
  msp_osd_config.overlay_radio_mode = 0;  //  0

  msp_osd_config.osd_rssi_value_pos = osd_rssi_value_pos;
  msp_osd_config.osd_main_batt_voltage_pos = osd_main_batt_voltage_pos;
  msp_osd_config.osd_crosshairs_pos = osd_crosshairs_pos;
  msp_osd_config.osd_artificial_horizon_pos = osd_artificial_horizon_pos;
  msp_osd_config.osd_horizon_sidebars_pos = osd_horizon_sidebars_pos;
  msp_osd_config.osd_item_timer_1_pos = osd_item_timer_1_pos;
  msp_osd_config.osd_item_timer_2_pos = osd_item_timer_2_pos;
  msp_osd_config.osd_flymode_pos = osd_flymode_pos;
  msp_osd_config.osd_craft_name_pos = osd_craft_name_pos;
  msp_osd_config.osd_throttle_pos_pos = osd_throttle_pos_pos;
  msp_osd_config.osd_vtx_channel_pos = osd_vtx_channel_pos;
  msp_osd_config.osd_current_draw_pos = osd_current_draw_pos;
  msp_osd_config.osd_mah_drawn_pos = osd_mah_drawn_pos;
  msp_osd_config.osd_gps_speed_pos = osd_gps_speed_pos;
  msp_osd_config.osd_gps_sats_pos = osd_gps_sats_pos;
  msp_osd_config.osd_altitude_pos = osd_altitude_pos;
  msp_osd_config.osd_roll_pids_pos = osd_roll_pids_pos;
  msp_osd_config.osd_pitch_pids_pos = osd_pitch_pids_pos;
  msp_osd_config.osd_yaw_pids_pos = osd_yaw_pids_pos;
  msp_osd_config.osd_power_pos = osd_power_pos;
  msp_osd_config.osd_pidrate_profile_pos = osd_pidrate_profile_pos;
  msp_osd_config.osd_warnings_pos = osd_warnings_pos;
  msp_osd_config.osd_avg_cell_voltage_pos = osd_avg_cell_voltage_pos;
  msp_osd_config.osd_gps_lon_pos = osd_gps_lon_pos;
  msp_osd_config.osd_gps_lat_pos = osd_gps_lat_pos;
  msp_osd_config.osd_debug_pos = osd_debug_pos;
  msp_osd_config.osd_pitch_angle_pos = osd_pitch_angle_pos;
  msp_osd_config.osd_roll_angle_pos = osd_roll_angle_pos;
  msp_osd_config.osd_main_batt_usage_pos = osd_main_batt_usage_pos;
  msp_osd_config.osd_disarmed_pos = osd_disarmed_pos;
  msp_osd_config.osd_home_dir_pos = osd_home_dir_pos;
  msp_osd_config.osd_home_dist_pos = osd_home_dist_pos;
  msp_osd_config.osd_numerical_heading_pos = osd_numerical_heading_pos;
  msp_osd_config.osd_numerical_vario_pos = osd_numerical_vario_pos;
  msp_osd_config.osd_compass_bar_pos = osd_compass_bar_pos;
  msp_osd_config.osd_esc_tmp_pos = osd_esc_tmp_pos;
  msp_osd_config.osd_esc_rpm_pos = osd_esc_rpm_pos;
  msp_osd_config.osd_remaining_time_estimate_pos = osd_remaining_time_estimate_pos;
  msp_osd_config.osd_rtc_datetime_pos = osd_rtc_datetime_pos;
  msp_osd_config.osd_adjustment_range_pos = osd_adjustment_range_pos;
  msp_osd_config.osd_core_temperature_pos = osd_core_temperature_pos;
  msp_osd_config.osd_anti_gravity_pos = osd_anti_gravity_pos;
  msp_osd_config.osd_g_force_pos = osd_g_force_pos;
  msp_osd_config.osd_motor_diag_pos = osd_motor_diag_pos;
  msp_osd_config.osd_log_status_pos = osd_log_status_pos;
  msp_osd_config.osd_flip_arrow_pos = osd_flip_arrow_pos;
  msp_osd_config.osd_link_quality_pos = osd_link_quality_pos;
  msp_osd_config.osd_flight_dist_pos = osd_flight_dist_pos;
  msp_osd_config.osd_stick_overlay_left_pos = osd_stick_overlay_left_pos;
  msp_osd_config.osd_stick_overlay_right_pos = osd_stick_overlay_right_pos;
  msp_osd_config.osd_display_name_pos = osd_display_name_pos;
  msp_osd_config.osd_esc_rpm_freq_pos = osd_esc_rpm_freq_pos;
  msp_osd_config.osd_rate_profile_name_pos = osd_rate_profile_name_pos;
  msp_osd_config.osd_pid_profile_name_pos = osd_pid_profile_name_pos;
  msp_osd_config.osd_profile_name_pos = osd_profile_name_pos;
  msp_osd_config.osd_rssi_dbm_value_pos = osd_rssi_dbm_value_pos;
  msp_osd_config.osd_rc_channels_pos = osd_rc_channels_pos;

  msp.send(MSP_OSD_CONFIG, &msp_osd_config, sizeof(msp_osd_config));
}


void invert_pos(uint16_t *pos1, uint16_t *pos2) {
  uint16_t tmp_pos = *pos1;
  *pos1 = *pos2;
  *pos2 = tmp_pos;
}

void set_flight_mode_flags(bool arm) {
    if ((flightModeFlags == 0x00000002) && arm) {
      flightModeFlags = 0x00000003;    // arm
      #ifdef DEBUG
        Serial.println("ARMING");
      #endif
    } else if ((flightModeFlags == 0x00000003) && !arm) {        
      flightModeFlags = 0x00000002;    // disarm 
      #ifdef DEBUG
        Serial.println("DISARMING");
      #endif
    }
}

void display_flight_mode() {
  show_text(&craftname);
}

void send_msp_to_airunit() {
  
  //MSP_FC_VARIANT
  memcpy(fc_variant.flightControlIdentifier, fcVariant, sizeof(fcVariant));
  msp.send(MSP_FC_VARIANT, &fc_variant, sizeof(fc_variant));

  //MSP_FC_VERSION
  fc_version.versionMajor = 4;
  fc_version.versionMinor = 1;
  fc_version.versionPatchLevel = 1;
  msp.send(MSP_FC_VERSION, &fc_version, sizeof(fc_version));

  //MSP_NAME
  memcpy(name.craft_name, craftname, sizeof(craftname));
  msp.send(MSP_NAME, &name, sizeof(name));

  //MSP_STATUS
  status_DJI.flightModeFlags = flightModeFlags;
  status_DJI.armingFlags = 0x0303;
  msp.send(MSP_STATUS_EX, &status_DJI, sizeof(status_DJI));
  status_DJI.armingFlags = 0x0000;
  msp.send(MSP_STATUS, &status_DJI, sizeof(status_DJI));

  //MSP_BATTERY_STATE
  battery_state.amperage = 0;
  battery_state.batteryVoltage = vbat * 10;
  battery_state.mAhDrawn = 0;
  battery_state.batteryCellCount = batteryCellCount;
  battery_state.batteryCapacity = 0;
  battery_state.batteryState = 0;
  battery_state.legacyBatteryVoltage = vbat;
  msp.send(MSP_BATTERY_STATE, &battery_state, sizeof(battery_state));
 
  //MSP_OSD_CONFIG
  send_osd_config();
}

void show_text(char (*text)[15]) {
  memcpy(craftname, *text, sizeof(craftname));
}

void set_battery_cells_number() {
  if (vbat < 43) batteryCellCount = 1;
  else if (vbat < 85) batteryCellCount = 2;
  else if (vbat < 127) batteryCellCount = 3;
  else if (vbat < 169) batteryCellCount = 4;
  else if (vbat < 211) batteryCellCount = 5;
  else if (vbat < 255) batteryCellCount = 6;
}



void getVoltageSample() {
  vbat = analogRead(ANALOG_IN)*10/VOLT_DIVIDER;
}

//*** USED ONLY FOR DEBUG ***
void debugPrint() {
  Serial.println("**********************************");
  Serial.print("Flight Mode: ");
  Serial.println(flightModeFlags);
  Serial.print("Voltage: ");
  Serial.println(((double)vbat / 10), 1);
  Serial.print("Sample Count / transmit: ");
  Serial.println(lastCount);
  Serial.print("Battery Cell Count: ");
  Serial.println(batteryCellCount);
}