From 0f02e12f4038d3efc1188daff651e86066e4506a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 17 Apr 2014 23:02:22 +0100 Subject: [PATCH] de-duplicate failsafe code. extract failsafe code to seperate files. remove dependency on board.h and mw.h on a few files. Moved rx configuration paramaters into rxConfig in order to remove the dependency on config_t from the sbus rx code - sumd/spektrum to follow. Avoided use of YAW/PITCH/ROLL in some files since those constants are from an unrelated enum type. Replaced some magic numbers with constants to remove comments and improve code readability. Note, due to the tight coupling and global variable usage it was difficult to create a smaller commit. --- Makefile | 3 + src/buzzer.c | 53 ++++++++--- src/buzzer.h | 3 + src/config.c | 36 ++----- src/config.h | 188 +++++-------------------------------- src/config_storage.h | 139 +++++++++++++++++++++++++++ src/drivers/pwm_common.c | 4 +- src/failsafe.c | 73 ++++++++++++++ src/failsafe.h | 10 ++ src/flight_imu.c | 8 +- src/flight_mixer.c | 9 +- src/gps_common.c | 1 + src/main.c | 14 +-- src/mw.c | 103 ++++---------------- src/mw.h | 65 +------------ src/runtime_config.c | 37 ++++++++ src/runtime_config.h | 72 ++++++++++---- src/rx_common.c | 59 ++++++++++++ src/rx_common.h | 42 +++++++++ src/rx_sbus.c | 32 +++++-- src/rx_sbus.h | 4 + src/rx_spektrum.c | 17 ++-- src/rx_sumd.c | 6 +- src/sensors_acceleration.c | 50 +++++----- src/serial_cli.c | 12 +-- src/typeconversion.c | 3 - 26 files changed, 606 insertions(+), 437 deletions(-) create mode 100644 src/buzzer.h create mode 100644 src/config_storage.h create mode 100644 src/failsafe.c create mode 100644 src/failsafe.h create mode 100644 src/runtime_config.c create mode 100644 src/rx_common.c create mode 100644 src/rx_sbus.h diff --git a/Makefile b/Makefile index 3a112775b..3d3b7d277 100644 --- a/Makefile +++ b/Makefile @@ -47,6 +47,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ boardalignment.c \ buzzer.c \ config.c \ + failsafe.c \ main.c \ mw.c \ printf.c \ @@ -68,6 +69,8 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ flight_imu.c \ flight_mixer.c \ gps_common.c \ + runtime_config.c \ + rx_common.c \ rx_sbus.c \ rx_sumd.c \ rx_spektrum.c \ diff --git a/src/buzzer.c b/src/buzzer.c index c13130743..2de97b262 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -1,16 +1,37 @@ -#include "board.h" -#include "mw.h" +#include "stdbool.h" +#include "stdint.h" + +#include "platform.h" +#include "drivers/sound_beeper.h" +#include "drivers/system_common.h" +#include "drivers/serial_common.h" // FIXME this file should not have a dependency on serial ports, see core_t from runtime_config.h +#include "failsafe.h" +#include "sensors_common.h" +#include "runtime_config.h" +#include "config.h" + +#include "buzzer.h" + +#define LONG_PAUSE_DELAY 50 static uint8_t buzzerIsOn = 0, beepDone = 0; static uint32_t buzzerLastToggleTime; static void beep(uint16_t pulse); static void beep_code(char first, char second, char third, char pause); +uint8_t toggleBeep = 0; + +typedef enum { + FAILSAFE_IDLE = 0, + FAILSAFE_LANDING, + FAILSAFE_FIND_ME +} failsafeBuzzerWarnings_e; + void buzzer(bool warn_vbat) { static uint8_t beeperOnBox; static uint8_t warn_noGPSfix = 0; - static uint8_t warn_failsafe = 0; + static failsafeBuzzerWarnings_e warn_failsafe = FAILSAFE_IDLE; static uint8_t warn_runtime = 0; //===================== BeeperOn via rcOptions ===================== @@ -21,15 +42,21 @@ void buzzer(bool warn_vbat) } //===================== Beeps for failsafe ===================== if (feature(FEATURE_FAILSAFE)) { - if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { - warn_failsafe = 1; //set failsafe warning level to 1 while landing - if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) - warn_failsafe = 2; //start "find me" signal after landing + if (shouldFailsafeForceLanding(f.ARMED)) { + warn_failsafe = FAILSAFE_LANDING; + + if (shouldFailsafeHaveCausedLandingByNow()) { + warn_failsafe = FAILSAFE_FIND_ME; + } + } + + if (hasFailsafeTimerElapsed() && !f.ARMED) { + warn_failsafe = FAILSAFE_FIND_ME; + } + + if (isFailsafeIdle()) { + warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay } - if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) - warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal - if (failsafeCnt == 0) - warn_failsafe = 0; // turn off alarm if TX is okay } //===================== GPS fix notification handling ===================== @@ -88,7 +115,7 @@ void beep_code(char first, char second, char third, char pause) Duration = 0; break; default: - Duration = 50; + Duration = LONG_PAUSE_DELAY; break; } @@ -109,7 +136,7 @@ void beep_code(char first, char second, char third, char pause) static void beep(uint16_t pulse) { - if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + 50))) { // Buzzer is off and long pause time is up -> turn it on + if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + LONG_PAUSE_DELAY))) { // Buzzer is off and long pause time is up -> turn it on buzzerIsOn = 1; BEEP_ON; buzzerLastToggleTime = millis(); // save the time the buzer turned on diff --git a/src/buzzer.h b/src/buzzer.h new file mode 100644 index 000000000..6218b56fb --- /dev/null +++ b/src/buzzer.h @@ -0,0 +1,3 @@ +#pragma once + +void buzzer(bool warn_vbat); diff --git a/src/config.c b/src/config.c index d747eba19..5116ee3e8 100755 --- a/src/config.c +++ b/src/config.c @@ -1,6 +1,6 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" -#include "config.h" #include @@ -8,6 +8,9 @@ #include "telemetry_common.h" #include "gps_common.h" +#include "config_storage.h" +#include "config.h" + #ifndef FLASH_PAGE_COUNT #define FLASH_PAGE_COUNT 128 @@ -21,7 +24,6 @@ config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; static const uint8_t EEPROM_CONF_VERSION = 62; -static uint32_t enabledSensors = 0; static void resetConf(void); void parseRcChannels(const char *input) @@ -31,7 +33,7 @@ void parseRcChannels(const char *input) for (c = input; *c; c++) { s = strchr(rcChannelLetters, *c); if (s) - mcfg.rcmap[s - rcChannelLetters] = c - input; + mcfg.rxConfig.rcmap[s - rcChannelLetters] = c - input; } } @@ -198,13 +200,13 @@ static void resetConf(void) mcfg.batteryConfig.vbatmaxcellvoltage = 43; mcfg.batteryConfig.vbatmincellvoltage = 33; mcfg.power_adc_channel = 0; - mcfg.serialrx_type = 0; mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY; mcfg.telemetry_port = TELEMETRY_PORT_UART; mcfg.telemetry_switch = 0; - mcfg.midrc = 1500; - mcfg.mincheck = 1100; - mcfg.maxcheck = 1900; + mcfg.rxConfig.serialrx_type = 0; + mcfg.rxConfig.midrc = 1500; + mcfg.rxConfig.mincheck = 1100; + mcfg.rxConfig.maxcheck = 1900; mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right mcfg.flaps_speed = 0; mcfg.fixedwing_althold_dir = 1; @@ -330,25 +332,6 @@ static void resetConf(void) memcpy(&mcfg.profile[i], &cfg, sizeof(config_t)); } -bool sensors(uint32_t mask) -{ - return enabledSensors & mask; -} - -void sensorsSet(uint32_t mask) -{ - enabledSensors |= mask; -} - -void sensorsClear(uint32_t mask) -{ - enabledSensors &= ~(mask); -} - -uint32_t sensorsMask(void) -{ - return enabledSensors; -} bool feature(uint32_t mask) { @@ -374,3 +357,4 @@ uint32_t featureMask(void) { return mcfg.enabledFeatures; } + diff --git a/src/config.h b/src/config.h index 481074630..3a92d9af1 100644 --- a/src/config.h +++ b/src/config.h @@ -17,171 +17,27 @@ enum { 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 enum { + FEATURE_PPM = 1 << 0, + FEATURE_VBAT = 1 << 1, + FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_SERIALRX = 1 << 3, + FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_SERVO_TILT = 1 << 5, + FEATURE_SOFTSERIAL = 1 << 6, + FEATURE_LED_RING = 1 << 7, + FEATURE_GPS = 1 << 8, + FEATURE_FAILSAFE = 1 << 9, + FEATURE_SONAR = 1 << 10, + FEATURE_TELEMETRY = 1 << 11, + FEATURE_POWERMETER = 1 << 12, + FEATURE_VARIO = 1 << 13, + FEATURE_3D = 1 << 14, +} AvailableFeatures; - -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 - boardAlignment_t boardAlignment; - 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; +bool feature(uint32_t mask); +void featureSet(uint32_t mask); +void featureClear(uint32_t mask); +void featureClearAll(void); +uint32_t featureMask(void); diff --git a/src/config_storage.h b/src/config_storage.h new file mode 100644 index 000000000..f610ba240 --- /dev/null +++ b/src/config_storage.h @@ -0,0 +1,139 @@ +#pragma once + +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[ANGLE_INDEX_COUNT]; // 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 + boardAlignment_t boardAlignment; + 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) + + rxConfig_t rxConfig; + 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.c b/src/drivers/pwm_common.c index 0e8232963..f2ba17f99 100755 --- a/src/drivers/pwm_common.c +++ b/src/drivers/pwm_common.c @@ -10,6 +10,8 @@ #include "timer_common.h" #include "pwm_common.h" +#include "failsafe.h" // FIXME for external global variables + /* Configuration maps: @@ -72,8 +74,6 @@ static uint8_t numMotors = 0; static uint8_t numServos = 0; static uint8_t numInputs = 0; static uint16_t failsafeThreshold = 985; -// external vars (ugh) -extern int16_t failsafeCnt; static const uint8_t multiPPM[] = { PWM1 | TYPE_IP, // PPM input diff --git a/src/failsafe.c b/src/failsafe.c new file mode 100644 index 000000000..bc0955b8f --- /dev/null +++ b/src/failsafe.c @@ -0,0 +1,73 @@ +#include +#include + +#include "rx_common.h" + + +#include "drivers/serial_common.h" // FIXME this file should not have a dependency on serial ports, see core_t from runtime_config.h +#include "flight_mixer.h" // FIXME this file should not have a dependency on the flight mixer, see config_t, servoParam_t, etc from config_storage.h +#include "axis.h" // FIXME this file should not have a dependency axis +#include "flight_common.h" // FIXME this file should not have a dependency on the flight common, see config_t from config_storage.h +#include "sensors_common.h" // FIXME this file should not have a dependency on the sensors, see sensor_align_e from config_storage.h +#include "boardalignment.h" // FIXME this file should not have a dependency on board alignment +#include "battery.h" // FIXME this file should not have a dependency on battery, see batteryConfig_t from config_storage.h + +#include "runtime_config.h" +#include "config.h" +#include "config_storage.h" + +int16_t failsafeCnt = 0; +int16_t failsafeEvents = 0; + +bool isFailsafeIdle(void) +{ + return failsafeCnt == 0; +} + +bool hasFailsafeTimerElapsed(void) +{ + return failsafeCnt > (5 * cfg.failsafe_delay); +} + +bool shouldFailsafeForceLanding(bool armed) +{ + return hasFailsafeTimerElapsed() && armed; +} + +bool shouldFailsafeHaveCausedLandingByNow(void) +{ + return failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay); +} + +void failsafeAvoidRearm(void) +{ + mwDisarm(); // This will prevent the automatic rearm if failsafe shuts it down and prevents + f.OK_TO_ARM = 0; // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm +} + + +void updateFailsafeState(void) +{ + uint8_t i; + + if (!feature(FEATURE_FAILSAFE)) { + return; + } + + if (hasFailsafeTimerElapsed()) { + + if (shouldFailsafeForceLanding(f.ARMED)) { // Stabilize, and set Throttle to specified level + for (i = 0; i < 3; i++) { + rcData[i] = mcfg.rxConfig.midrc; // after specified guard time after RC signal is lost (in 0.1sec) + } + rcData[THROTTLE] = cfg.failsafe_throttle; + failsafeEvents++; + } + + if (shouldFailsafeHaveCausedLandingByNow() || !f.ARMED) { + failsafeAvoidRearm(); + } + } + failsafeCnt++; +} + diff --git a/src/failsafe.h b/src/failsafe.h new file mode 100644 index 000000000..d7deea42a --- /dev/null +++ b/src/failsafe.h @@ -0,0 +1,10 @@ +#pragma once + +extern int16_t failsafeCnt; +extern int16_t failsafeEvents; + +bool isFailsafeIdle(void); +bool hasFailsafeTimerElapsed(void); +bool shouldFailsafeForceLanding(bool armed); +bool shouldFailsafeHaveCausedLandingByNow(void); +void updateFailsafeState(void); diff --git a/src/flight_imu.c b/src/flight_imu.c index f41d280bb..45098d531 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -1,10 +1,10 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" #include "maths.h" #include "sensors_compass.h" -#include "flight_common.h" int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT]; @@ -167,9 +167,9 @@ void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta) // deprecated - it uses legacy indices for ROLL/PITCH/YAW, see rc_alias_e - use rotateAnglesV instead void rotateV(struct fp_vector *v, float *delta) { fp_angles_t temp; - temp.roll = delta[ROLL]; - temp.pitch = delta[PITCH]; - temp.yaw = delta[YAW]; + temp.roll = delta[GI_ROLL]; + temp.pitch = delta[GI_PITCH]; + temp.yaw = delta[GI_YAW]; rotateAnglesV(v, &temp); } diff --git a/src/flight_mixer.c b/src/flight_mixer.c index ac95129b3..4eb6ddafc 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -1,7 +1,8 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" -#include "flight_common.h" +#include "rx_common.h" #include "maths.h" @@ -351,14 +352,14 @@ static void airplaneMixer(void) int16_t lFlap = servoMiddle(2); lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); - lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle? + lFlap = mcfg.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle? if (slow_LFlaps < lFlap) slow_LFlaps += mcfg.flaps_speed; else if (slow_LFlaps > lFlap) slow_LFlaps -= mcfg.flaps_speed; servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += mcfg.midrc; + servo[2] += mcfg.rxConfig.midrc; } if (f.PASSTHRU_MODE) { // Direct passthru from RX @@ -498,7 +499,7 @@ void mixTable(void) } } else { motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); - if ((rcData[THROTTLE]) < mcfg.mincheck) { + if ((rcData[THROTTLE]) < mcfg.rxConfig.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) motor[i] = mcfg.minthrottle; else diff --git a/src/gps_common.c b/src/gps_common.c index 59e7d9896..02ab05aba 100644 --- a/src/gps_common.c +++ b/src/gps_common.c @@ -1,4 +1,5 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" #include "maths.h" diff --git a/src/main.c b/src/main.c index 1cf84bc2f..feb2d8dac 100755 --- a/src/main.c +++ b/src/main.c @@ -1,10 +1,13 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" -#include "rx.h" +#include "rx_sbus.h" +#include "rx_common.h" #include "telemetry_common.h" #include "boardalignment.h" #include "config.h" +#include "config_storage.h" #include "build_config.h" @@ -12,9 +15,6 @@ core_t core; extern rcReadRawDataPtr rcReadRawFunc; -// receiver read function -extern uint16_t pwmReadRawRC(uint8_t chan); - int main(void) { uint8_t i; @@ -63,7 +63,7 @@ int main(void) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = mcfg.midrc; + pwm_params.servoCenterPulse = mcfg.rxConfig.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: @@ -86,13 +86,13 @@ int main(void) core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { - switch (mcfg.serialrx_type) { + switch (mcfg.rxConfig.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: - sbusInit(&rcReadRawFunc); + sbusInit(&rcReadRawFunc, &mcfg.rxConfig); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); diff --git a/src/mw.c b/src/mw.c index 43b2befbf..7a0ef3944 100755 --- a/src/mw.c +++ b/src/mw.c @@ -1,18 +1,18 @@ #include "board.h" +#include "flight_common.h" #include "mw.h" #include "serial_cli.h" #include "telemetry_common.h" -#include "flight_common.h" #include "typeconversion.h" -#include "rx.h" +#include "rx_common.h" +#include "rx_sbus.h" +#include "failsafe.h" #include "maths.h" // June 2013 V2.2-dev -flags_t f; int16_t debug[4]; -uint8_t toggleBeep = 0; uint32_t currentTime = 0; 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 @@ -20,21 +20,16 @@ int16_t headFreeModeHold; int16_t telemTemperature1; // gyro sensor temperature -int16_t failsafeCnt = 0; -int16_t failsafeEvents = 0; -int16_t rcData[RC_CHANS]; // interval [1000;2000] int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE uint16_t rssi; // range: [0;1023] -rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) static void pidMultiWii(void); 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[CHECKBOX_ITEM_COUNT]; int16_t axisPID[3]; @@ -104,7 +99,7 @@ void annexCode(void) } for (axis = 0; axis < 3; axis++) { - tmp = min(abs(rcData[axis] - mcfg.midrc), 500); + tmp = min(abs(rcData[axis] - mcfg.rxConfig.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { @@ -132,12 +127,12 @@ void annexCode(void) dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; - if (rcData[axis] < mcfg.midrc) + if (rcData[axis] < mcfg.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } - tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); - tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] + tmp = constrain(rcData[THROTTLE], mcfg.rxConfig.mincheck, 2000); + tmp = (uint32_t)(tmp - mcfg.rxConfig.mincheck) * 1000 / (2000 - mcfg.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -213,44 +208,6 @@ void annexCode(void) } } -uint16_t pwmReadRawRC(uint8_t chan) -{ - uint16_t data; - - data = pwmRead(mcfg.rcmap[chan]); - if (data < 750 || data > 2250) - data = mcfg.midrc; - - return data; -} - -void computeRC(void) -{ - uint8_t chan; - - if (feature(FEATURE_SERIALRX)) { - for (chan = 0; chan < 8; chan++) - rcData[chan] = rcReadRawFunc(chan); - } else { - static int16_t rcData4Values[8][4], rcDataMean[8]; - static uint8_t rc4ValuesIndex = 0; - uint8_t a; - - rc4ValuesIndex++; - for (chan = 0; chan < 8; chan++) { - rcData4Values[chan][rc4ValuesIndex % 4] = rcReadRawFunc(chan); - rcDataMean[chan] = 0; - for (a = 0; a < 4; a++) - rcDataMean[chan] += rcData4Values[chan][a]; - - rcDataMean[chan] = (rcDataMean[chan] + 2) / 4; - if (rcDataMean[chan] < rcData[chan] - 3) - rcData[chan] = rcDataMean[chan] + 2; - if (rcDataMean[chan] > rcData[chan] + 3) - rcData[chan] = rcDataMean[chan] - 2; - } - } -} static void mwArm(void) { @@ -266,12 +223,6 @@ static void mwArm(void) } } -static void mwDisarm(void) -{ - if (f.ARMED) - f.ARMED = 0; -} - static void mwVario(void) { @@ -439,7 +390,7 @@ void loop(void) // calculate rc stuff from serial-based receivers (spek/sbus) if (feature(FEATURE_SERIALRX)) { - switch (mcfg.serialrx_type) { + switch (mcfg.rxConfig.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: rcReady = spektrumFrameComplete(); @@ -456,7 +407,7 @@ void loop(void) if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven rcReady = false; rcTime = currentTime + 20000; - computeRC(); + computeRC(&mcfg.rxConfig); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { @@ -472,33 +423,15 @@ void loop(void) rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); } - // Failsafe routine - if (feature(FEATURE_FAILSAFE)) { - if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level - for (i = 0; i < 3; i++) - rcData[i] = mcfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec) - rcData[THROTTLE] = cfg.failsafe_throttle; - if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec) - mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents - f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm - } - failsafeEvents++; - } - if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm - mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents - f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm - } - failsafeCnt++; - } - // end of failsafe routine - next change is made with RcOptions setting + updateFailsafeState(); // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; - if (rcData[i] > mcfg.mincheck) + if (rcData[i] > mcfg.rxConfig.mincheck) stTmp |= 0x80; // check for MIN - if (rcData[i] < mcfg.maxcheck) + if (rcData[i] < mcfg.rxConfig.maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { @@ -509,9 +442,9 @@ void loop(void) rcSticks = stTmp; // perform actions - if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.midrc + mcfg.deadband3d_throttle))) + if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.rxConfig.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.rxConfig.midrc + mcfg.deadband3d_throttle))) isThrottleLow = true; - else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.mincheck)) + else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.rxConfig.mincheck)) isThrottleLow = true; if (isThrottleLow) { errorGyroI[ROLL] = 0; @@ -610,7 +543,7 @@ void loop(void) } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { - if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement + if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = false; } @@ -630,8 +563,8 @@ void loop(void) 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 - if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) { + // note: if FAILSAFE is disable, hasFailsafeTimerElapsed() is always false + if ((rcOptions[BOXANGLE] || hasFailsafeTimerElapsed()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode if (!f.ANGLE_MODE) { errorAngleI[ROLL] = 0; diff --git a/src/mw.h b/src/mw.h index f6115a7c5..56d472c0f 100755 --- a/src/mw.h +++ b/src/mw.h @@ -1,6 +1,7 @@ #pragma once #include "runtime_config.h" +#include "flight_common.h" /* for VBAT monitoring frequency */ #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations @@ -11,8 +12,6 @@ #define LAT 0 #define LON 1 -#define RC_CHANS (18) - // Serial GPS only variables // navigation mode typedef enum NavigationMode @@ -56,31 +55,6 @@ typedef enum GimbalFlags { GIMBAL_FORWARDAUX = 1 << 2, } GimbalFlags; -/*********** RC alias *****************/ -typedef enum rc_alias { - ROLL = 0, - PITCH, - YAW, - THROTTLE, - AUX1, - AUX2, - AUX3, - AUX4 -} rc_alias_e; - -#define ROL_LO (1 << (2 * ROLL)) -#define ROL_CE (3 << (2 * ROLL)) -#define ROL_HI (2 << (2 * ROLL)) -#define PIT_LO (1 << (2 * PITCH)) -#define PIT_CE (3 << (2 * PITCH)) -#define PIT_HI (2 << (2 * PITCH)) -#define YAW_LO (1 << (2 * YAW)) -#define YAW_CE (3 << (2 * YAW)) -#define YAW_HI (2 << (2 * YAW)) -#define THR_LO (1 << (2 * THROTTLE)) -#define THR_CE (3 << (2 * THROTTLE)) -#define THR_HI (2 << (2 * THROTTLE)) - #include "flight_mixer.h" enum { @@ -93,31 +67,12 @@ enum { #define CALIBRATING_ACC_CYCLES 400 #define CALIBRATING_BARO_CYCLES 200 +#include "rx_common.h" #include "config.h" - -typedef struct flags_t { - uint8_t OK_TO_ARM; - uint8_t ARMED; - uint8_t ACC_CALIBRATED; - uint8_t ANGLE_MODE; - uint8_t HORIZON_MODE; - uint8_t MAG_MODE; - uint8_t BARO_MODE; - uint8_t GPS_HOME_MODE; - uint8_t GPS_HOLD_MODE; - uint8_t HEADFREE_MODE; - uint8_t PASSTHRU_MODE; - uint8_t GPS_FIX; - uint8_t GPS_FIX_HOME; - uint8_t SMALL_ANGLES_25; - uint8_t CALIBRATE_MAG; - uint8_t VARIO_MODE; - uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code -} flags_t; +#include "config_storage.h" extern int16_t axisPID[3]; extern int16_t rcCommand[4]; -extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; extern int16_t failsafeCnt; extern int16_t debug[4]; @@ -146,7 +101,6 @@ extern int16_t headFreeModeHold; extern int16_t heading, magHold; extern int16_t motor[MAX_MOTORS]; extern int16_t servo[MAX_SERVOS]; -extern int16_t rcData[RC_CHANS]; extern uint16_t rssi; // range: [0;1023] extern uint8_t vbat; extern int16_t telemTemperature1; // gyro sensor temperature @@ -221,24 +175,11 @@ void parseRcChannels(const char *input); void readEEPROM(void); void writeEEPROM(uint8_t b, uint8_t updateProfile); void checkFirstTime(bool reset); -bool sensors(uint32_t mask); -void sensorsSet(uint32_t mask); -void sensorsClear(uint32_t mask); -uint32_t sensorsMask(void); -bool feature(uint32_t mask); -void featureSet(uint32_t mask); -void featureClear(uint32_t mask); -void featureClearAll(void); -uint32_t featureMask(void); // spektrum void spektrumInit(rcReadRawDataPtr *callback); bool spektrumFrameComplete(void); -// sbus -void sbusInit(rcReadRawDataPtr *callback); -bool sbusFrameComplete(void); - // sumd void sumdInit(rcReadRawDataPtr *callback); bool sumdFrameComplete(void); diff --git a/src/runtime_config.c b/src/runtime_config.c new file mode 100644 index 000000000..be106362a --- /dev/null +++ b/src/runtime_config.c @@ -0,0 +1,37 @@ +#include +#include + +#include "drivers/serial_common.h" + +#include "runtime_config.h" + +flags_t f; +uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; + +static uint32_t enabledSensors = 0; + +bool sensors(uint32_t mask) +{ + return enabledSensors & mask; +} + +void sensorsSet(uint32_t mask) +{ + enabledSensors |= mask; +} + +void sensorsClear(uint32_t mask) +{ + enabledSensors &= ~(mask); +} + +uint32_t sensorsMask(void) +{ + return enabledSensors; +} + +void mwDisarm(void) +{ + if (f.ARMED) + f.ARMED = 0; +} diff --git a/src/runtime_config.h b/src/runtime_config.h index 73b49bbb8..4f4e0668c 100644 --- a/src/runtime_config.h +++ b/src/runtime_config.h @@ -1,22 +1,51 @@ #pragma once -typedef enum { - FEATURE_PPM = 1 << 0, - FEATURE_VBAT = 1 << 1, - FEATURE_INFLIGHT_ACC_CAL = 1 << 2, - FEATURE_SERIALRX = 1 << 3, - FEATURE_MOTOR_STOP = 1 << 4, - FEATURE_SERVO_TILT = 1 << 5, - FEATURE_SOFTSERIAL = 1 << 6, - FEATURE_LED_RING = 1 << 7, - FEATURE_GPS = 1 << 8, - FEATURE_FAILSAFE = 1 << 9, - FEATURE_SONAR = 1 << 10, - FEATURE_TELEMETRY = 1 << 11, - FEATURE_POWERMETER = 1 << 12, - FEATURE_VARIO = 1 << 13, - FEATURE_3D = 1 << 14, -} AvailableFeatures; +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 +}; + +extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; + +typedef struct flags_t { + uint8_t OK_TO_ARM; + uint8_t ARMED; + uint8_t ACC_CALIBRATED; + uint8_t ANGLE_MODE; + uint8_t HORIZON_MODE; + uint8_t MAG_MODE; + uint8_t BARO_MODE; + uint8_t GPS_HOME_MODE; + uint8_t GPS_HOLD_MODE; + uint8_t HEADFREE_MODE; + uint8_t PASSTHRU_MODE; + uint8_t GPS_FIX; + uint8_t GPS_FIX_HOME; + uint8_t SMALL_ANGLES_25; + uint8_t CALIBRATE_MAG; + uint8_t VARIO_MODE; + uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code +} flags_t; // Core runtime settings typedef struct core_t { @@ -29,7 +58,14 @@ typedef struct core_t { bool useServo; // feature SERVO_TILT or wing/airplane mixers will enable this } core_t; -typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype extern core_t core; +extern flags_t f; + +bool sensors(uint32_t mask); +void sensorsSet(uint32_t mask); +void sensorsClear(uint32_t mask); +uint32_t sensorsMask(void); + +void mwDisarm(void); diff --git a/src/rx_common.c b/src/rx_common.c new file mode 100644 index 000000000..120ecf5c8 --- /dev/null +++ b/src/rx_common.c @@ -0,0 +1,59 @@ +#include +#include +#include + +#include "platform.h" + +#include "rx_common.h" +#include "config.h" + +// FIXME all includes below are just for pwmReadRawRC because it uses mcfg +#ifdef FY90Q +#include "drivers/pwm_fy90q.h" +#else +#include "drivers/pwm_common.h" +#endif + + +int16_t rcData[RC_CHANS]; // interval [1000;2000] + +rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) + +uint16_t pwmReadRawRC(rxConfig_t *rxConfig, uint8_t chan) +{ + uint16_t data; + + data = pwmRead(rxConfig->rcmap[chan]); + if (data < 750 || data > 2250) + data = rxConfig->midrc; + + return data; +} + +void computeRC(rxConfig_t *rxConfig) +{ + uint8_t chan; + + if (feature(FEATURE_SERIALRX)) { + for (chan = 0; chan < 8; chan++) + rcData[chan] = rcReadRawFunc(rxConfig, chan); + } else { + static int16_t rcData4Values[8][4], rcDataMean[8]; + static uint8_t rc4ValuesIndex = 0; + uint8_t a; + + rc4ValuesIndex++; + for (chan = 0; chan < 8; chan++) { + rcData4Values[chan][rc4ValuesIndex % 4] = rcReadRawFunc(rxConfig, chan); + rcDataMean[chan] = 0; + for (a = 0; a < 4; a++) + rcDataMean[chan] += rcData4Values[chan][a]; + + rcDataMean[chan] = (rcDataMean[chan] + 2) / 4; + if (rcDataMean[chan] < rcData[chan] - 3) + rcData[chan] = rcDataMean[chan] + 2; + if (rcDataMean[chan] > rcData[chan] + 3) + rcData[chan] = rcDataMean[chan] - 2; + } + } +} diff --git a/src/rx_common.h b/src/rx_common.h index ae48d91b1..085cf57c4 100644 --- a/src/rx_common.h +++ b/src/rx_common.h @@ -1,8 +1,50 @@ #pragma once +typedef enum rc_alias { + ROLL = 0, + PITCH, + YAW, + THROTTLE, + AUX1, + AUX2, + AUX3, + AUX4 +} rc_alias_e; + +#define ROL_LO (1 << (2 * ROLL)) +#define ROL_CE (3 << (2 * ROLL)) +#define ROL_HI (2 << (2 * ROLL)) +#define PIT_LO (1 << (2 * PITCH)) +#define PIT_CE (3 << (2 * PITCH)) +#define PIT_HI (2 << (2 * PITCH)) +#define YAW_LO (1 << (2 * YAW)) +#define YAW_CE (3 << (2 * YAW)) +#define YAW_HI (2 << (2 * YAW)) +#define THR_LO (1 << (2 * THROTTLE)) +#define THR_CE (3 << (2 * THROTTLE)) +#define THR_HI (2 << (2 * THROTTLE)) + typedef enum { SERIALRX_SPEKTRUM1024 = 0, SERIALRX_SPEKTRUM2048 = 1, SERIALRX_SBUS = 2, SERIALRX_SUMD = 3, } SerialRXType; + +#define RC_CHANS (18) + +extern int16_t rcData[RC_CHANS]; // interval [1000;2000] + +typedef struct rxConfig_s { + 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 +} rxConfig_t; + +typedef uint16_t (* rcReadRawDataPtr)(rxConfig_t *rxConfig, uint8_t chan); // used by receiver driver to return channel data + +uint16_t pwmReadRawRC(rxConfig_t *rxConfig, uint8_t chan); +void computeRC(rxConfig_t *rxConfig); + diff --git a/src/rx_sbus.c b/src/rx_sbus.c index 7fda225dc..0a6fa600b 100644 --- a/src/rx_sbus.c +++ b/src/rx_sbus.c @@ -1,5 +1,17 @@ -#include "board.h" -#include "mw.h" +#include +#include + +#include "platform.h" + +#include "rx_common.h" + +#include "drivers/system_common.h" + +#include "drivers/serial_common.h" +#include "drivers/serial_uart.h" +#include "runtime_config.h" + +#include "rx_sbus.h" // driver for SBUS receiver using UART2 @@ -10,18 +22,23 @@ static bool sbusFrameDone = false; static void sbusDataReceive(uint16_t c); -static uint16_t sbusReadRawRC(uint8_t chan); +static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, uint8_t chan); // external vars (ugh) extern int16_t failsafeCnt; static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; -void sbusInit(rcReadRawDataPtr *callback) +//rxConfig_t *rxConfig; + +void sbusInit(rcReadRawDataPtr *callback, rxConfig_t *rxConfig) { int b; + + //rxConfig = initialRxConfig; + for (b = 0; b < SBUS_MAX_CHANNEL; b++) - sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET); + sbusChannelData[b] = 2 * (rxConfig->midrc - SBUS_OFFSET); core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS)); if (callback) *callback = sbusReadRawRC; @@ -101,7 +118,8 @@ bool sbusFrameComplete(void) return false; } -static uint16_t sbusReadRawRC(uint8_t chan) +static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, uint8_t chan) { - return sbusChannelData[mcfg.rcmap[chan]] / 2 + SBUS_OFFSET; + return sbusChannelData[rxConfig->rcmap[chan]] / 2 + SBUS_OFFSET; } + diff --git a/src/rx_sbus.h b/src/rx_sbus.h new file mode 100644 index 000000000..ec2ea846d --- /dev/null +++ b/src/rx_sbus.h @@ -0,0 +1,4 @@ +#pragma once + +void sbusInit(rcReadRawDataPtr *callback, rxConfig_t *initialRxConfig); +bool sbusFrameComplete(void); diff --git a/src/rx_spektrum.c b/src/rx_spektrum.c index c076e9cbf..30025f490 100644 --- a/src/rx_spektrum.c +++ b/src/rx_spektrum.c @@ -1,27 +1,30 @@ #include "board.h" #include "mw.h" -#include "rx.h" +#include "rx_common.h" // driver for spektrum satellite receiver / sbus using UART2 (freeing up more motor outputs for stuff) #define SPEK_MAX_CHANNEL 7 #define SPEK_FRAME_SIZE 16 + static uint8_t spek_chan_shift; static uint8_t spek_chan_mask; static bool rcFrameComplete = false; static bool spekHiRes = false; static bool spekDataIncoming = false; + volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; + static void spektrumDataReceive(uint16_t c); -static uint16_t spektrumReadRawRC(uint8_t chan); +static uint16_t spektrumReadRawRC(rxConfig_t *rxConfig, uint8_t chan); // external vars (ugh) extern int16_t failsafeCnt; void spektrumInit(rcReadRawDataPtr *callback) { - switch (mcfg.serialrx_type) { + switch (mcfg.rxConfig.serialrx_type) { case SERIALRX_SPEKTRUM2048: // 11 bit frames spek_chan_shift = 3; @@ -69,7 +72,7 @@ bool spektrumFrameComplete(void) return rcFrameComplete; } -static uint16_t spektrumReadRawRC(uint8_t chan) +static uint16_t spektrumReadRawRC(rxConfig_t*rxConfig, uint8_t chan) { uint16_t data; static uint32_t spekChannelData[SPEK_MAX_CHANNEL]; @@ -85,12 +88,12 @@ static uint16_t spektrumReadRawRC(uint8_t chan) } if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) { - data = mcfg.midrc; + data = rxConfig->midrc; } else { if (spekHiRes) - data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode + data = 988 + (spekChannelData[rxConfig->rcmap[chan]] >> 1); // 2048 mode else - data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode + data = 988 + spekChannelData[rxConfig->rcmap[chan]]; // 1024 mode } return data; diff --git a/src/rx_sumd.c b/src/rx_sumd.c index 77bcfe8ea..4c006a857 100644 --- a/src/rx_sumd.c +++ b/src/rx_sumd.c @@ -9,7 +9,7 @@ static bool sumdFrameDone = false; static void sumdDataReceive(uint16_t c); -static uint16_t sumdReadRawRC(uint8_t chan); +static uint16_t sumdReadRawRC(rxConfig_t *rxConfig, uint8_t chan); static uint32_t sumdChannelData[SUMD_MAX_CHANNEL]; @@ -70,7 +70,7 @@ bool sumdFrameComplete(void) return false; } -static uint16_t sumdReadRawRC(uint8_t chan) +static uint16_t sumdReadRawRC(rxConfig_t *rxConfig, uint8_t chan) { - return sumdChannelData[mcfg.rcmap[chan]] / 8; + return sumdChannelData[rxConfig->rcmap[chan]] / 8; } diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index b5e006770..e605e8f36 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -1,8 +1,10 @@ #include "board.h" -#include "mw.h" #include "flight_common.h" +#include "mw.h" + + uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; @@ -29,11 +31,11 @@ void ACC_Common(void) } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (calibratingA == 1) { - mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; - cfg.angleTrim[ROLL] = 0; - cfg.angleTrim[PITCH] = 0; + mcfg.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + mcfg.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + mcfg.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; + cfg.angleTrim[AI_ROLL] = 0; + cfg.angleTrim[AI_PITCH] = 0; writeEEPROM(1, true); // write accZero in EEPROM } calibratingA--; @@ -45,11 +47,11 @@ void ACC_Common(void) static int16_t angleTrim_saved[2] = { 0, 0 }; // Saving old zeropoints before measurement if (InflightcalibratingA == 50) { - accZero_saved[ROLL] = mcfg.accZero[ROLL]; - accZero_saved[PITCH] = mcfg.accZero[PITCH]; - accZero_saved[YAW] = mcfg.accZero[YAW]; - angleTrim_saved[ROLL] = cfg.angleTrim[ROLL]; - angleTrim_saved[PITCH] = cfg.angleTrim[PITCH]; + accZero_saved[GI_ROLL] = mcfg.accZero[GI_ROLL]; + accZero_saved[GI_PITCH] = mcfg.accZero[GI_PITCH]; + accZero_saved[GI_YAW] = mcfg.accZero[GI_YAW]; + angleTrim_saved[AI_ROLL] = cfg.angleTrim[AI_ROLL]; + angleTrim_saved[AI_PITCH] = cfg.angleTrim[AI_PITCH]; } if (InflightcalibratingA > 0) { for (axis = 0; axis < 3; axis++) { @@ -68,29 +70,29 @@ void ACC_Common(void) AccInflightCalibrationMeasurementDone = true; toggleBeep = 2; // buzzer for indicatiing the end of calibration // recover saved values to maintain current flight behavior until new values are transferred - mcfg.accZero[ROLL] = accZero_saved[ROLL]; - mcfg.accZero[PITCH] = accZero_saved[PITCH]; - mcfg.accZero[YAW] = accZero_saved[YAW]; - cfg.angleTrim[ROLL] = angleTrim_saved[ROLL]; - cfg.angleTrim[PITCH] = angleTrim_saved[PITCH]; + mcfg.accZero[GI_ROLL] = accZero_saved[GI_ROLL]; + mcfg.accZero[GI_PITCH] = accZero_saved[GI_PITCH]; + mcfg.accZero[GI_YAW] = accZero_saved[GI_YAW]; + cfg.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL]; + cfg.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH]; } InflightcalibratingA--; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again AccInflightCalibrationSavetoEEProm = false; - mcfg.accZero[ROLL] = b[ROLL] / 50; - mcfg.accZero[PITCH] = b[PITCH] / 50; - mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G - cfg.angleTrim[ROLL] = 0; - cfg.angleTrim[PITCH] = 0; + mcfg.accZero[GI_ROLL] = b[GI_ROLL] / 50; + mcfg.accZero[GI_PITCH] = b[GI_PITCH] / 50; + mcfg.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G + cfg.angleTrim[AI_ROLL] = 0; + cfg.angleTrim[AI_PITCH] = 0; writeEEPROM(1, true); // write accZero in EEPROM } } - accADC[ROLL] -= mcfg.accZero[ROLL]; - accADC[PITCH] -= mcfg.accZero[PITCH]; - accADC[YAW] -= mcfg.accZero[YAW]; + accADC[GI_ROLL] -= mcfg.accZero[GI_ROLL]; + accADC[GI_PITCH] -= mcfg.accZero[GI_PITCH]; + accADC[GI_YAW] -= mcfg.accZero[GI_YAW]; } void ACC_getADC(void) diff --git a/src/serial_cli.c b/src/serial_cli.c index 12a05af06..b1deb16a0 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -121,12 +121,12 @@ typedef struct { const clivalue_t valueTable[] = { { "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 }, - { "midrc", VAR_UINT16, &mcfg.midrc, 1200, 1700 }, + { "midrc", VAR_UINT16, &mcfg.rxConfig.midrc, 1200, 1700 }, { "minthrottle", VAR_UINT16, &mcfg.minthrottle, 0, 2000 }, { "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, 0, 2000 }, { "mincommand", VAR_UINT16, &mcfg.mincommand, 0, 2000 }, - { "mincheck", VAR_UINT16, &mcfg.mincheck, 0, 2000 }, - { "maxcheck", VAR_UINT16, &mcfg.maxcheck, 0, 2000 }, + { "mincheck", VAR_UINT16, &mcfg.rxConfig.mincheck, 0, 2000 }, + { "maxcheck", VAR_UINT16, &mcfg.rxConfig.maxcheck, 0, 2000 }, { "deadband3d_low", VAR_UINT16, &mcfg.deadband3d_low, 0, 2000 }, { "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, 0, 2000 }, { "neutral3d", VAR_UINT16, &mcfg.neutral3d, 0, 2000 }, @@ -143,7 +143,7 @@ const clivalue_t valueTable[] = { { "softserial_2_inverted", VAR_UINT8, &mcfg.softserial_2_inverted, 0, 1 }, { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, GPS_HARDWARE_MAX }, { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, GPS_BAUD_MAX }, - { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 3 }, + { "serialrx_type", VAR_UINT8, &mcfg.rxConfig.serialrx_type, 0, 3 }, { "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 }, @@ -615,7 +615,7 @@ static void cliDump(char *cmdline) // print RC MAPPING for (i = 0; i < 8; i++) - buf[mcfg.rcmap[i]] = rcChannelLetters[i]; + buf[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; printf("map %s\r\n", buf); @@ -735,7 +735,7 @@ static void cliMap(char *cmdline) } cliPrint("Current assignment: "); for (i = 0; i < 8; i++) - out[mcfg.rcmap[i]] = rcChannelLetters[i]; + out[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[i] = '\0'; printf("%s\r\n", out); } diff --git a/src/typeconversion.c b/src/typeconversion.c index 376dd3726..7918e353d 100644 --- a/src/typeconversion.c +++ b/src/typeconversion.c @@ -1,6 +1,3 @@ -#include "board.h" -#include "mw.h" - #include "build_config.h" #ifdef REQUIRE_PRINTF_LONG_SUPPORT