rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Dominic Clifton 2014-04-17 23:02:22 +01:00
parent d7eb416aa9
commit 0f02e12f40
26 changed files with 606 additions and 437 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -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 \

53
src/buzzer.c
View File

@ -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

3
src/buzzer.h Normal file
View File

@ -0,0 +1,3 @@
#pragma once
void buzzer(bool warn_vbat);

36
src/config.c
View File

@ -1,6 +1,6 @@
#include "board.h"
#include "flight_common.h"
#include "mw.h"
#include "config.h"
#include <string.h>
@ -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;
}

188
src/config.h
View File

@ -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);

139
src/config_storage.h Normal file
View File

@ -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;

4
src/drivers/pwm_common.c
View File

@ -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

73
src/failsafe.c Normal file
View File

@ -0,0 +1,73 @@
#include <stdbool.h>
#include <stdint.h>
#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++;
}

10
src/failsafe.h Normal file
View File

@ -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);

8
src/flight_imu.c
View File

@ -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);
}

9
src/flight_mixer.c
View File

@ -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

1
src/gps_common.c
View File

@ -1,4 +1,5 @@
#include "board.h"
#include "flight_common.h"
#include "mw.h"
#include "maths.h"

14
src/main.c
View File

@ -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);

103
src/mw.c
View File

@ -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;

65
src/mw.h
View File

@ -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);

37
src/runtime_config.c Normal file
View File

@ -0,0 +1,37 @@
#include <stdbool.h>
#include <stdint.h>
#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;
}

72
src/runtime_config.h
View File

@ -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);

59
src/rx_common.c Normal file
View File

@ -0,0 +1,59 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#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;
}
}
}

42
src/rx_common.h
View File

@ -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);

32
src/rx_sbus.c
View File

@ -1,5 +1,17 @@
#include "board.h"
#include "mw.h"
#include <stdbool.h>
#include <stdint.h>
#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;
}

4
src/rx_sbus.h Normal file
View File

@ -0,0 +1,4 @@
#pragma once
void sbusInit(rcReadRawDataPtr *callback, rxConfig_t *initialRxConfig);
bool sbusFrameComplete(void);

17
src/rx_spektrum.c
View File

@ -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;

6
src/rx_sumd.c
View File

@ -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;
}

50
src/sensors_acceleration.c
View File

@ -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)

12
src/serial_cli.c
View File

@ -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);
}

3
src/typeconversion.c
View File

@ -1,6 +1,3 @@
#include "board.h"
#include "mw.h"
#include "build_config.h"
#ifdef REQUIRE_PRINTF_LONG_SUPPORT