diff --git a/Makefile b/Makefile index 534a66d449..3a112775bf 100644 --- a/Makefile +++ b/Makefile @@ -42,6 +42,7 @@ BIN_DIR = $(ROOT)/obj # Source files common to all targets COMMON_SRC = startup_stm32f10x_md_gcc.S \ + build_config.c \ battery.c \ boardalignment.c \ buzzer.c \ diff --git a/src/battery.c b/src/battery.c index 2a9b4c4733..25ba612d58 100644 --- a/src/battery.c +++ b/src/battery.c @@ -1,19 +1,53 @@ -#include "board.h" -#include "mw.h" +#include "stdbool.h" +#include "stdint.h" + +#include "drivers/adc_common.h" +#include "drivers/system_common.h" + +#include "battery.h" // Battery monitoring stuff uint8_t batteryCellCount = 3; // cell count uint16_t batteryWarningVoltage; // annoying buzzer after this one, battery ready to be dead +uint8_t vbat; // battery voltage in 0.1V steps + +static batteryConfig_t *batteryConfig; + uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return (((src) * 3.3f) / 4095) * mcfg.vbatscale; + return (((src) * 3.3f) / 4095) * batteryConfig->vbatscale; } -void batteryInit(void) + +void updateBatteryVoltage(void) { + static uint16_t vbatSamples[8]; + static uint8_t currentSampleIndex = 0; + uint8_t index; + uint16_t vbatSampleTotal = 0; + + // store the battery voltage with some other recent battery voltage readings + vbatSamples[(currentSampleIndex++) % 8] = adcGetChannel(ADC_BATTERY); + + // calculate vbat based on the average of recent readings + for (index = 0; index < 8; index++) { + vbatSampleTotal += vbatSamples[index]; + } + vbat = batteryAdcToVoltage(vbatSampleTotal / 8); +} + +bool shouldSoundBatteryAlarm(void) +{ + return !((vbat > batteryWarningVoltage) || (vbat < batteryConfig->vbatmincellvoltage)); +} + +void batteryInit(batteryConfig_t *initialBatteryConfig) +{ + batteryConfig = initialBatteryConfig; + uint32_t i; uint32_t voltage = 0; @@ -27,9 +61,10 @@ void batteryInit(void) // autodetect cell count, going from 2S..6S for (i = 2; i < 6; i++) { - if (voltage < i * mcfg.vbatmaxcellvoltage) + if (voltage < i * batteryConfig->vbatmaxcellvoltage) break; } batteryCellCount = i; - batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI + batteryWarningVoltage = i * batteryConfig->vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI } + diff --git a/src/battery.h b/src/battery.h index 6b6041bd79..36d3e84b6c 100644 --- a/src/battery.h +++ b/src/battery.h @@ -1,5 +1,15 @@ +#pragma once + +typedef struct batteryConfig_s { + uint8_t vbatscale; // adjust this to match battery voltage to reported value + uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) + uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V) +} batteryConfig_t; extern uint16_t batteryWarningVoltage; uint16_t batteryAdcToVoltage(uint16_t src); -void batteryInit(void); +bool shouldSoundBatteryAlarm(void); +void updateBatteryVoltage(void); +void batteryInit(batteryConfig_t *initialBatteryConfig); + diff --git a/src/build_config.c b/src/build_config.c new file mode 100644 index 0000000000..f4940a04f8 --- /dev/null +++ b/src/build_config.c @@ -0,0 +1,20 @@ +#include "stdbool.h" +#include "stdint.h" + +#include "platform.h" + +#include "drivers/gpio_common.h" +#include "drivers/timer_common.h" +#include "drivers/pwm_common.h" +#include "flight_mixer.h" +#include "sensors_common.h" +#include "battery.h" +#include "config.h" + +#if MAX_MOTORS != MAX_SUPPORTED_MOTORS +#error Motor configuration mismatch +#endif + +#if MAX_SERVOS != MAX_SUPPORTED_SERVOS +#error Servo configuration mismatch +#endif diff --git a/src/buzzer.c b/src/buzzer.c index 56bbc40090..c131307436 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -6,7 +6,7 @@ static uint32_t buzzerLastToggleTime; static void beep(uint16_t pulse); static void beep_code(char first, char second, char third, char pause); -void buzzer(uint8_t warn_vbat) +void buzzer(bool warn_vbat) { static uint8_t beeperOnBox; static uint8_t warn_noGPSfix = 0; @@ -52,12 +52,8 @@ void buzzer(uint8_t warn_vbat) beep_code('S','S','N','M'); else if (beeperOnBox == 1) beep_code('S','S','S','M'); // beeperon - else if (warn_vbat == 4) + else if (warn_vbat) beep_code('S','M','M','D'); - else if (warn_vbat == 2) - beep_code('S','S','M','D'); - else if (warn_vbat == 1) - beep_code('S','M','N','D'); else if (warn_runtime == 1 && f.ARMED) beep_code('S','S','M','N'); // Runtime warning else if (toggleBeep > 0) diff --git a/src/config.c b/src/config.c index f2e0b60f01..ef7cd9e519 100755 --- a/src/config.c +++ b/src/config.c @@ -1,5 +1,6 @@ #include "board.h" #include "mw.h" +#include "config.h" #include @@ -193,9 +194,9 @@ static void resetConf(void) mcfg.max_angle_inclination = 500; // 50 degrees mcfg.yaw_control_direction = 1; mcfg.moron_threshold = 32; - mcfg.vbatscale = 110; - mcfg.vbatmaxcellvoltage = 43; - mcfg.vbatmincellvoltage = 33; + mcfg.batteryConfig.vbatscale = 110; + mcfg.batteryConfig.vbatmaxcellvoltage = 43; + mcfg.batteryConfig.vbatmincellvoltage = 33; mcfg.power_adc_channel = 0; mcfg.serialrx_type = 0; mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY; diff --git a/src/config.h b/src/config.h new file mode 100644 index 0000000000..089dbd9130 --- /dev/null +++ b/src/config.h @@ -0,0 +1,189 @@ +#pragma once + +#define MAX_SUPPORTED_MOTORS 12 +#define MAX_SUPPORTED_SERVOS 8 + +enum { + PIDROLL, + PIDPITCH, + PIDYAW, + PIDALT, + PIDPOS, + PIDPOSR, + PIDNAVR, + PIDLEVEL, + PIDMAG, + PIDVEL, + PID_ITEM_COUNT +}; + +enum { + BOXARM = 0, + BOXANGLE, + BOXHORIZON, + BOXBARO, + BOXVARIO, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, + BOXCAMSTAB, + BOXCAMTRIG, + BOXGPSHOME, + BOXGPSHOLD, + BOXPASSTHRU, + BOXBEEPERON, + BOXLEDMAX, + BOXLEDLOW, + BOXLLIGHTS, + BOXCALIB, + BOXGOV, + BOXOSD, + BOXTELEMETRY, + CHECKBOX_ITEM_COUNT +}; + + +typedef struct config_t { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 + uint8_t P8[PID_ITEM_COUNT]; + uint8_t I8[PID_ITEM_COUNT]; + uint8_t D8[PID_ITEM_COUNT]; + + uint8_t rcRate8; + uint8_t rcExpo8; + uint8_t thrMid8; + uint8_t thrExpo8; + + uint8_t rollPitchRate; + uint8_t yawRate; + + uint8_t dynThrPID; + uint16_t tpaBreakPoint; // Breakpoint where TPA is activated + int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ + int16_t angleTrim[2]; // accelerometer trim + + // sensor-related stuff + uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter + uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations + uint8_t accxy_deadband; // set the acc deadband for xy-Axis + uint8_t baro_tab_size; // size of baro filter array + float baro_noise_lpf; // additional LPF to reduce baro noise + float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + float baro_cf_alt; // apply CF to use ACC for height estimation + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off + + uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches + + // Radio/ESC-related configuration + uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. + uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. + uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 + uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement + uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg + uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. + + // Servo-related stuff + servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration + + // Failsafe related configuration + uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) + uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) + uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. + uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe. + + // mixer-related configuration + int8_t yaw_direction; + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + + // gimbal-related configuration + uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff + + // gps-related stuff + uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) + uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) + uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes + uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it + uint16_t nav_speed_min; // cm/sec + uint16_t nav_speed_max; // cm/sec + uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS +} config_t; + +// System-wide +typedef struct master_t { + uint8_t version; + uint16_t size; + uint8_t magic_be; // magic number, should be 0xBE + + uint8_t mixerConfiguration; + uint32_t enabledFeatures; + uint16_t looptime; // imu loop time in us + motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; // custom mixtable + + // motor/esc/servo related stuff + uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. + uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 + uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint16_t deadband3d_low; // min 3d value + uint16_t deadband3d_high; // max 3d value + uint16_t neutral3d; // center 3d value + uint16_t deadband3d_throttle; // default throttle deadband from MIDRC + uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) + uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) + + // global sensor-related stuff + sensor_align_e gyro_align; // gyro alignment + sensor_align_e acc_align; // acc alignment + sensor_align_e mag_align; // mag alignment + int16_t board_align_roll; // board alignment correction in roll (deg) + int16_t board_align_pitch; // board alignment correction in pitch (deg) + int16_t board_align_yaw; // board alignment correction in yaw (deg) + int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) + uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. + uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter + uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). + int16_t accZero[3]; + int16_t magZero[3]; + + batteryConfig_t batteryConfig; + + uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9) + + // Radio/ESC-related configuration + uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order + uint8_t serialrx_type; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_SERIALRX first. + uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here + uint16_t mincheck; // minimum rc end + uint16_t maxcheck; // maximum rc end + uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right + uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster + int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign + + uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. + + // gps-related stuff + uint8_t gps_type; // See GPSHardware enum. + int8_t gps_baudrate; // See GPSBaudRates enum. + + uint32_t serial_baudrate; + + uint32_t softserial_baudrate; // shared by both soft serial ports + uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 + uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2 + + uint8_t telemetry_provider; // See TelemetryProvider enum. + uint8_t telemetry_port; // See TelemetryPort enum. + uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. + config_t profile[3]; // 3 separate profiles + uint8_t current_profile; // currently loaded profile + uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. + + uint8_t magic_ef; // magic number, should be 0xEF + uint8_t chk; // XOR checksum +} master_t; + +extern master_t mcfg; +extern config_t cfg; + diff --git a/src/drivers/pwm_common.h b/src/drivers/pwm_common.h index 8b597ca5b7..c8e4b42e4a 100755 --- a/src/drivers/pwm_common.h +++ b/src/drivers/pwm_common.h @@ -3,6 +3,7 @@ #define MAX_MOTORS 12 #define MAX_SERVOS 8 #define MAX_INPUTS 8 + #define PULSE_1MS (1000) // 1ms pulse width typedef struct drv_pwm_config_t { diff --git a/src/flight_mixer.h b/src/flight_mixer.h new file mode 100644 index 0000000000..735d155ccb --- /dev/null +++ b/src/flight_mixer.h @@ -0,0 +1,23 @@ +#pragma once + +// Custom mixer data per motor +typedef struct motorMixer_t { + float throttle; + float roll; + float pitch; + float yaw; +} motorMixer_t; + +// Custom mixer configuration +typedef struct mixer_t { + uint8_t numberMotor; + uint8_t useServo; + const motorMixer_t *motor; +} mixer_t; + +typedef struct servoParam_t { + int16_t min; // servo min + int16_t max; // servo max + int16_t middle; // servo middle + int8_t rate; // range [-100;+100] ; can be used to ajust a rate 0-100% and a direction +} servoParam_t; diff --git a/src/main.c b/src/main.c index 00b0e84fb7..105901cc83 100755 --- a/src/main.c +++ b/src/main.c @@ -145,7 +145,7 @@ int main(void) // Check battery type/voltage if (feature(FEATURE_VBAT)) - batteryInit(); + batteryInit(&mcfg.batteryConfig); serialInit(mcfg.serial_baudrate); diff --git a/src/mw.c b/src/mw.c index 2ac563703e..fb6d4ec063 100755 --- a/src/mw.c +++ b/src/mw.c @@ -18,7 +18,6 @@ uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop int16_t headFreeModeHold; -uint8_t vbat; // battery voltage in 0.1V steps int16_t telemTemperature1; // gyro sensor temperature int16_t failsafeCnt = 0; @@ -35,7 +34,7 @@ static void pidRewrite(void); pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii uint8_t dynP8[3], dynI8[3], dynD8[3]; -uint8_t rcOptions[CHECKBOXITEMS]; +uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; int16_t axisPID[3]; @@ -88,15 +87,10 @@ void annexCode(void) static uint32_t calibratedAccTime; int32_t tmp, tmp2; int32_t axis, prop1, prop2; - static uint8_t buzzerFreq; // delay between buzzer ring - // vbat shit + static uint8_t batteryWarningEnabled = false; + static uint8_t vbatTimer = 0; - static uint8_t ind = 0; - uint16_t vbatRaw = 0; - static uint16_t vbatRawArray[8]; - - int i; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < cfg.tpaBreakPoint) { @@ -158,18 +152,13 @@ void annexCode(void) if (feature(FEATURE_VBAT)) { if (!(++vbatTimer % VBATFREQ)) { - vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY); - for (i = 0; i < 8; i++) - vbatRaw += vbatRawArray[i]; - vbat = batteryAdcToVoltage(vbatRaw / 8); + updateBatteryVoltage(); + + batteryWarningEnabled = shouldSoundBatteryAlarm(); } - if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off - buzzerFreq = 0; - } else - buzzerFreq = 4; // low battery } - buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now + buzzer(batteryWarningEnabled); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; @@ -638,7 +627,7 @@ void loop(void) // Check AUX switches for (i = 0; i < 4; i++) auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2); - for (i = 0; i < CHECKBOXITEMS; i++) + for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) rcOptions[i] = (auxState & cfg.activate[i]) > 0; // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false diff --git a/src/mw.h b/src/mw.h index 1c04b8def4..f6115a7c52 100755 --- a/src/mw.h +++ b/src/mw.h @@ -68,45 +68,6 @@ typedef enum rc_alias { AUX4 } rc_alias_e; -enum { - PIDROLL, - PIDPITCH, - PIDYAW, - PIDALT, - PIDPOS, - PIDPOSR, - PIDNAVR, - PIDLEVEL, - PIDMAG, - PIDVEL, - PIDITEMS -}; - -enum { - BOXARM = 0, - BOXANGLE, - BOXHORIZON, - BOXBARO, - BOXVARIO, - BOXMAG, - BOXHEADFREE, - BOXHEADADJ, - BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, - BOXBEEPERON, - BOXLEDMAX, - BOXLEDLOW, - BOXLLIGHTS, - BOXCALIB, - BOXGOV, - BOXOSD, - BOXTELEMETRY, - CHECKBOXITEMS -}; - #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL)) @@ -120,27 +81,7 @@ enum { #define THR_CE (3 << (2 * THROTTLE)) #define THR_HI (2 << (2 * THROTTLE)) -// Custom mixer data per motor -typedef struct motorMixer_t { - float throttle; - float roll; - float pitch; - float yaw; -} motorMixer_t; - -// Custom mixer configuration -typedef struct mixer_t { - uint8_t numberMotor; - uint8_t useServo; - const motorMixer_t *motor; -} mixer_t; - -typedef struct servoParam_t { - int16_t min; // servo min - int16_t max; // servo max - int16_t middle; // servo middle - int8_t rate; // range [-100;+100] ; can be used to ajust a rate 0-100% and a direction -} servoParam_t; +#include "flight_mixer.h" enum { ALIGN_GYRO = 0, @@ -152,148 +93,7 @@ enum { #define CALIBRATING_ACC_CYCLES 400 #define CALIBRATING_BARO_CYCLES 200 -typedef struct config_t { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 - uint8_t P8[PIDITEMS]; - uint8_t I8[PIDITEMS]; - uint8_t D8[PIDITEMS]; - - uint8_t rcRate8; - uint8_t rcExpo8; - uint8_t thrMid8; - uint8_t thrExpo8; - - uint8_t rollPitchRate; - uint8_t yawRate; - - uint8_t dynThrPID; - uint16_t tpaBreakPoint; // Breakpoint where TPA is activated - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ - int16_t angleTrim[2]; // accelerometer trim - - // sensor-related stuff - uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter - uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations - uint8_t accxy_deadband; // set the acc deadband for xy-Axis - uint8_t baro_tab_size; // size of baro filter array - float baro_noise_lpf; // additional LPF to reduce baro noise - float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) - float baro_cf_alt; // apply CF to use ACC for height estimation - uint8_t acc_unarmedcal; // turn automatic acc compensation on/off - - uint16_t activate[CHECKBOXITEMS]; // activate switches - - // Radio/ESC-related configuration - uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. - uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. - uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 - uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement - uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg - uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. - - // Servo-related stuff - servoParam_t servoConf[8]; // servo configuration - - // Failsafe related configuration - uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) - uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) - uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. - uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe. - - // mixer-related configuration - int8_t yaw_direction; - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - - // gimbal-related configuration - uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff - - // gps-related stuff - uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) - uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) - uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes - uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it - uint16_t nav_speed_min; // cm/sec - uint16_t nav_speed_max; // cm/sec - uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS -} config_t; - -// System-wide -typedef struct master_t { - uint8_t version; - uint16_t size; - uint8_t magic_be; // magic number, should be 0xBE - - uint8_t mixerConfiguration; - uint32_t enabledFeatures; - uint16_t looptime; // imu loop time in us - motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable - - // motor/esc/servo related stuff - uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. - uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 - uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t deadband3d_low; // min 3d value - uint16_t deadband3d_high; // max 3d value - uint16_t neutral3d; // center 3d value - uint16_t deadband3d_throttle; // default throttle deadband from MIDRC - uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) - uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) - - // global sensor-related stuff - sensor_align_e gyro_align; // gyro alignment - sensor_align_e acc_align; // acc alignment - sensor_align_e mag_align; // mag alignment - int16_t board_align_roll; // board alignment correction in roll (deg) - int16_t board_align_pitch; // board alignment correction in pitch (deg) - int16_t board_align_yaw; // board alignment correction in yaw (deg) - int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) - uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device - uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. - uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. - uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter - uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). - int16_t accZero[3]; - int16_t magZero[3]; - - // Battery/ADC stuff - uint8_t vbatscale; // adjust this to match battery voltage to reported value - uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) - uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V) - uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9) - - // Radio/ESC-related configuration - uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order - uint8_t serialrx_type; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_SERIALRX first. - uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here - uint16_t mincheck; // minimum rc end - uint16_t maxcheck; // maximum rc end - uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right - uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster - int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign - - uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. - - // gps-related stuff - uint8_t gps_type; // See GPSHardware enum. - int8_t gps_baudrate; // See GPSBaudRates enum. - - uint32_t serial_baudrate; - - uint32_t softserial_baudrate; // shared by both soft serial ports - uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 - uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2 - - uint8_t telemetry_provider; // See TelemetryProvider enum. - uint8_t telemetry_port; // See TelemetryPort enum. - uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. - config_t profile[3]; // 3 separate profiles - uint8_t current_profile; // currently loaded profile - uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. - - uint8_t magic_ef; // magic number, should be 0xEF - uint8_t chk; // XOR checksum -} master_t; +#include "config.h" typedef struct flags_t { uint8_t OK_TO_ARM; @@ -317,7 +117,7 @@ typedef struct flags_t { extern int16_t axisPID[3]; extern int16_t rcCommand[4]; -extern uint8_t rcOptions[CHECKBOXITEMS]; +extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; extern int16_t failsafeCnt; extern int16_t debug[4]; @@ -377,8 +177,6 @@ extern uint8_t GPS_svinfo_svid[16]; // Satellite ID extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) -extern master_t mcfg; -extern config_t cfg; extern flags_t f; extern sensor_t acc; extern sensor_t gyro; @@ -397,8 +195,6 @@ int getEstimatedAltitude(void); // Sensors void sensorsAutodetect(void); -void batteryInit(void); -uint16_t batteryAdcToVoltage(uint16_t src); void ACC_getADC(void); int Baro_update(void); void Gyro_getADC(void); @@ -448,7 +244,7 @@ void sumdInit(rcReadRawDataPtr *callback); bool sumdFrameComplete(void); // buzzer -void buzzer(uint8_t warn_vbat); +void buzzer(bool warn_vbat); void systemBeep(bool onoff); // cli diff --git a/src/serial_cli.c b/src/serial_cli.c index 3c3e379011..f61dad1ed1 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -151,9 +151,9 @@ const clivalue_t valueTable[] = { { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX }, { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 }, - { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, - { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 }, - { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 }, + { "vbatscale", VAR_UINT8, &mcfg.batteryConfig.vbatscale, 10, 200 }, + { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmaxcellvoltage, 10, 50 }, + { "vbatmincellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmincellvoltage, 10, 50 }, { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 }, { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 }, { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 }, @@ -447,17 +447,17 @@ static void cliAux(char *cmdline) len = strlen(cmdline); if (len == 0) { // print out aux channel settings - for (i = 0; i < CHECKBOXITEMS; i++) + for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) printf("aux %u %u\r\n", i, cfg.activate[i]); } else { ptr = cmdline; i = atoi(ptr); - if (i < CHECKBOXITEMS) { + if (i < CHECKBOX_ITEM_COUNT) { ptr = strchr(cmdline, ' '); val = atoi(ptr); cfg.activate[i] = val; } else { - printf("Invalid Feature index: must be < %u\r\n", CHECKBOXITEMS); + printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT); } } } diff --git a/src/serial_msp.c b/src/serial_msp.c index ea18b7b7f3..77aa72e7d5 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -93,11 +93,11 @@ struct box_t { { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, - { CHECKBOXITEMS, NULL, 0xFF } + { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; // this is calculated at startup based on enabled features. -static uint8_t availableBoxes[CHECKBOXITEMS]; +static uint8_t availableBoxes[CHECKBOX_ITEM_COUNT]; // this is the number of filled indexes in above array static uint8_t numberBoxItems = 0; // from mixer.c @@ -305,7 +305,7 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_SET_PID: - for (i = 0; i < PIDITEMS; i++) { + for (i = 0; i < PID_ITEM_COUNT; i++) { cfg.P8[i] = read8(); cfg.I8[i] = read8(); cfg.D8[i] = read8(); @@ -336,9 +336,9 @@ static void evaluateCommand(void) read16(); read32(); cfg.mag_declination = read16() * 10; - mcfg.vbatscale = read8(); // actual vbatscale as intended - mcfg.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI - mcfg.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + mcfg.batteryConfig.vbatscale = read8(); // actual vbatscale as intended + mcfg.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + mcfg.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI read8(); // vbatlevel_crit (unused) headSerialReply(0); break; @@ -488,8 +488,8 @@ static void evaluateCommand(void) serialize8(cfg.thrExpo8); break; case MSP_PID: - headSerialReply(3 * PIDITEMS); - for (i = 0; i < PIDITEMS; i++) { + headSerialReply(3 * PID_ITEM_COUNT); + for (i = 0; i < PID_ITEM_COUNT; i++) { serialize8(cfg.P8[i]); serialize8(cfg.I8[i]); serialize8(cfg.D8[i]); @@ -523,9 +523,9 @@ static void evaluateCommand(void) serialize16(0); // plog useless shit serialize32(0); // plog useless shit serialize16(cfg.mag_declination / 10); // TODO check this shit - serialize8(mcfg.vbatscale); - serialize8(mcfg.vbatmincellvoltage); - serialize8(mcfg.vbatmaxcellvoltage); + serialize8(mcfg.batteryConfig.vbatscale); + serialize8(mcfg.batteryConfig.vbatmincellvoltage); + serialize8(mcfg.batteryConfig.vbatmaxcellvoltage); serialize8(0); break; case MSP_MOTOR_PINS: