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