mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 14:25:20 +03:00
Removing final dependency on board.h.
board.h is no-longer required.
This commit is contained in:
parent
cb63f6e2b5
commit
92f74ac0c2
9 changed files with 83 additions and 199 deletions
86
src/board.h
86
src/board.h
|
@ -1,86 +0,0 @@
|
||||||
/*
|
|
||||||
* This file is deprecated. All this code belongs elsewhere - create appropriate headers and include them.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#define __USE_C99_MATH // for roundf()
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#include <ctype.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
|
|
||||||
#include "common/axis.h"
|
|
||||||
|
|
||||||
#include "sensors_common.h"
|
|
||||||
#include "drivers/accgyro_common.h"
|
|
||||||
#include "drivers/gpio_common.h"
|
|
||||||
#include "drivers/system_common.h"
|
|
||||||
#include "drivers/barometer_common.h"
|
|
||||||
#include "sensors_common.h"
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
#include "drivers/light_led.h"
|
|
||||||
#include "drivers/sound_beeper.h"
|
|
||||||
#include "boardalignment.h"
|
|
||||||
#include "battery.h"
|
|
||||||
|
|
||||||
#ifdef FY90Q
|
|
||||||
// FY90Q
|
|
||||||
#include "drivers/accgyro_fy90q.h"
|
|
||||||
#include "drivers/adc_common.h"
|
|
||||||
#include "drivers/adc_fy90q.h"
|
|
||||||
#include "drivers/bus_i2c.h"
|
|
||||||
#include "drivers/bus_spi.h"
|
|
||||||
#include "drivers/pwm_common.h"
|
|
||||||
#include "drivers/timer_common.h"
|
|
||||||
#include "drivers/serial_common.h"
|
|
||||||
#include "drivers/serial_uart.h"
|
|
||||||
#else
|
|
||||||
|
|
||||||
#ifdef OLIMEXINO
|
|
||||||
// OLIMEXINO
|
|
||||||
#include "drivers/adc_common.h"
|
|
||||||
#include "drivers/bus_i2c.h"
|
|
||||||
#include "drivers/bus_spi.h"
|
|
||||||
#include "drivers/accgyro_adxl345.h"
|
|
||||||
#include "drivers/accgyro_mpu3050.h"
|
|
||||||
#include "drivers/accgyro_mpu6050.h"
|
|
||||||
#include "drivers/accgyro_l3g4200d.h"
|
|
||||||
#include "drivers/pwm_common.h"
|
|
||||||
#include "drivers/timer_common.h"
|
|
||||||
#include "drivers/serial_common.h"
|
|
||||||
#include "drivers/serial_uart.h"
|
|
||||||
#include "drivers/serial_softserial.h"
|
|
||||||
#else
|
|
||||||
|
|
||||||
// AfroFlight32
|
|
||||||
#include "drivers/adc_common.h"
|
|
||||||
#include "drivers/accgyro_adxl345.h"
|
|
||||||
#include "drivers/accgyro_bma280.h"
|
|
||||||
#include "drivers/barometer_bmp085.h"
|
|
||||||
#include "drivers/barometer_ms5611.h"
|
|
||||||
#include "drivers/compass_hmc5883l.h"
|
|
||||||
#include "drivers/bus_i2c.h"
|
|
||||||
#include "drivers/bus_spi.h"
|
|
||||||
#include "drivers/light_ledring.h"
|
|
||||||
#include "drivers/accgyro_mma845x.h"
|
|
||||||
#include "drivers/accgyro_mpu3050.h"
|
|
||||||
#include "drivers/accgyro_mpu6050.h"
|
|
||||||
#include "drivers/accgyro_l3g4200d.h"
|
|
||||||
#include "drivers/pwm_common.h"
|
|
||||||
#include "drivers/timer_common.h"
|
|
||||||
#include "drivers/serial_common.h"
|
|
||||||
#include "drivers/serial_uart.h"
|
|
||||||
#include "drivers/serial_softserial.h"
|
|
||||||
#include "drivers/sonar_hcsr04.h"
|
|
||||||
|
|
||||||
#endif
|
|
||||||
#endif
|
|
8
src/flight_imu.h
Normal file
8
src/flight_imu.h
Normal file
|
@ -0,0 +1,8 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
extern int32_t errorAltitudeI;
|
||||||
|
extern int32_t BaroPID;
|
||||||
|
extern int16_t throttleAngleCorrection;
|
||||||
|
|
||||||
|
int getEstimatedAltitude(void);
|
||||||
|
void computeIMU(void);
|
|
@ -81,6 +81,9 @@ void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers);
|
||||||
void writeAllMotors(int16_t mc);
|
void writeAllMotors(int16_t mc);
|
||||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||||
void mixerResetMotors(void);
|
void mixerResetMotors(void);
|
||||||
|
void mixTable(void);
|
||||||
|
void writeServos(void);
|
||||||
|
void writeMotors(void);
|
||||||
|
|
||||||
// from mixer.c
|
// from mixer.c
|
||||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||||
|
|
87
src/mw.c
87
src/mw.c
|
@ -1,28 +1,60 @@
|
||||||
#include "board.h"
|
#include <stdbool.h>
|
||||||
#include "mw.h"
|
#include <stdlib.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/typeconversion.h"
|
#include "common/axis.h"
|
||||||
|
|
||||||
|
#include "drivers/accgyro_common.h"
|
||||||
|
#include "drivers/light_ledring.h"
|
||||||
|
#include "drivers/light_led.h"
|
||||||
|
#include "drivers/gpio_common.h"
|
||||||
|
#include "drivers/system_common.h"
|
||||||
|
#include "drivers/serial_common.h"
|
||||||
|
|
||||||
|
#include "boardalignment.h"
|
||||||
|
#include "battery.h"
|
||||||
#include "buzzer.h"
|
#include "buzzer.h"
|
||||||
#include "sensors_sonar.h"
|
#include "escservo.h"
|
||||||
#include "sensors_gyro.h"
|
|
||||||
#include "sensors_compass.h"
|
|
||||||
#include "sensors_barometer.h"
|
|
||||||
#include "sensors_acceleration.h"
|
|
||||||
#include "flight_common.h"
|
|
||||||
#include "serial_cli.h"
|
|
||||||
#include "telemetry_common.h"
|
|
||||||
#include "gps_common.h"
|
|
||||||
#include "rx_common.h"
|
|
||||||
#include "rx_sbus.h"
|
|
||||||
#include "rx_sumd.h"
|
|
||||||
#include "rx_spektrum.h"
|
|
||||||
#include "failsafe.h"
|
#include "failsafe.h"
|
||||||
|
#include "flight_imu.h"
|
||||||
|
#include "flight_common.h"
|
||||||
|
#include "flight_mixer.h"
|
||||||
|
#include "gimbal.h"
|
||||||
|
#include "gps_common.h"
|
||||||
|
#include "sensors_common.h"
|
||||||
|
#include "sensors_sonar.h"
|
||||||
|
#include "sensors_compass.h"
|
||||||
|
#include "sensors_acceleration.h"
|
||||||
|
#include "sensors_barometer.h"
|
||||||
|
#include "sensors_gyro.h"
|
||||||
|
#include "serial_cli.h"
|
||||||
|
#include "serial_common.h"
|
||||||
#include "statusindicator.h"
|
#include "statusindicator.h"
|
||||||
|
#include "rc_controls.h"
|
||||||
|
#include "rc_curves.h"
|
||||||
|
#include "rx_common.h"
|
||||||
|
#include "telemetry_common.h"
|
||||||
|
|
||||||
|
#include "runtime_config.h"
|
||||||
|
#include "config.h"
|
||||||
|
#include "config_profile.h"
|
||||||
|
#include "config_master.h"
|
||||||
|
|
||||||
// June 2013 V2.2-dev
|
// June 2013 V2.2-dev
|
||||||
|
|
||||||
|
enum {
|
||||||
|
ALIGN_GYRO = 0,
|
||||||
|
ALIGN_ACCEL = 1,
|
||||||
|
ALIGN_MAG = 2
|
||||||
|
};
|
||||||
|
|
||||||
|
/* for VBAT monitoring frequency */
|
||||||
|
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
||||||
|
|
||||||
int16_t debug[4];
|
int16_t debug[4];
|
||||||
uint32_t currentTime = 0;
|
uint32_t currentTime = 0;
|
||||||
uint32_t previousTime = 0;
|
uint32_t previousTime = 0;
|
||||||
|
@ -125,7 +157,7 @@ void annexCode(void)
|
||||||
|
|
||||||
buzzer(batteryWarningEnabled); // 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
|
if ((!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete())) { // Calibration phasis
|
||||||
LED0_TOGGLE;
|
LED0_TOGGLE;
|
||||||
} else {
|
} else {
|
||||||
if (f.ACC_CALIBRATED)
|
if (f.ACC_CALIBRATED)
|
||||||
|
@ -177,7 +209,7 @@ void annexCode(void)
|
||||||
|
|
||||||
static void mwArm(void)
|
static void mwArm(void)
|
||||||
{
|
{
|
||||||
if (calibratingG == 0 && f.ACC_CALIBRATED) {
|
if (isGyroCalibrationComplete() && f.ACC_CALIBRATED) {
|
||||||
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
|
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
|
||||||
// TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
|
// TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
|
||||||
if (!f.ARMED) { // arm now!
|
if (!f.ARMED) { // arm now!
|
||||||
|
@ -211,18 +243,7 @@ void loop(void)
|
||||||
|
|
||||||
// calculate rc stuff from serial-based receivers (spek/sbus)
|
// calculate rc stuff from serial-based receivers (spek/sbus)
|
||||||
if (feature(FEATURE_SERIALRX)) {
|
if (feature(FEATURE_SERIALRX)) {
|
||||||
switch (masterConfig.rxConfig.serialrx_type) {
|
rcReady = isSerialRxFrameComplete(&masterConfig.rxConfig);
|
||||||
case SERIALRX_SPEKTRUM1024:
|
|
||||||
case SERIALRX_SPEKTRUM2048:
|
|
||||||
rcReady = spektrumFrameComplete();
|
|
||||||
break;
|
|
||||||
case SERIALRX_SBUS:
|
|
||||||
rcReady = sbusFrameComplete();
|
|
||||||
break;
|
|
||||||
case SERIALRX_SUMD:
|
|
||||||
rcReady = sumdFrameComplete();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
|
if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
|
||||||
|
@ -292,11 +313,13 @@ void loop(void)
|
||||||
i = 0;
|
i = 0;
|
||||||
// GYRO calibration
|
// GYRO calibration
|
||||||
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
||||||
calibratingG = CALIBRATING_GYRO_CYCLES;
|
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||||||
if (feature(FEATURE_GPS))
|
if (feature(FEATURE_GPS))
|
||||||
GPS_reset_home_position();
|
GPS_reset_home_position();
|
||||||
|
#ifdef BARO
|
||||||
if (sensors(SENSOR_BARO))
|
if (sensors(SENSOR_BARO))
|
||||||
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
|
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
|
||||||
|
#endif
|
||||||
if (!sensors(SENSOR_MAG))
|
if (!sensors(SENSOR_MAG))
|
||||||
heading = 0; // reset heading to zero after gyro calibration
|
heading = 0; // reset heading to zero after gyro calibration
|
||||||
// Inflight ACC Calibration
|
// Inflight ACC Calibration
|
||||||
|
|
79
src/mw.h
79
src/mw.h
|
@ -1,79 +0,0 @@
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "rc_controls.h"
|
|
||||||
#include "escservo.h"
|
|
||||||
#include "rc_curves.h"
|
|
||||||
#include "runtime_config.h"
|
|
||||||
#include "flight_common.h"
|
|
||||||
#include "failsafe.h"
|
|
||||||
|
|
||||||
/* for VBAT monitoring frequency */
|
|
||||||
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
|
||||||
|
|
||||||
#include "gimbal.h"
|
|
||||||
#include "flight_mixer.h"
|
|
||||||
|
|
||||||
enum {
|
|
||||||
ALIGN_GYRO = 0,
|
|
||||||
ALIGN_ACCEL = 1,
|
|
||||||
ALIGN_MAG = 2
|
|
||||||
};
|
|
||||||
|
|
||||||
#include "sensors_barometer.h"
|
|
||||||
#include "sensors_gyro.h"
|
|
||||||
#include "serial_common.h"
|
|
||||||
#include "rx_common.h"
|
|
||||||
#include "gps_common.h"
|
|
||||||
#include "telemetry_common.h"
|
|
||||||
#include "config.h"
|
|
||||||
#include "config_profile.h"
|
|
||||||
#include "config_master.h"
|
|
||||||
|
|
||||||
extern int16_t debug[4];
|
|
||||||
extern uint16_t acc_1G;
|
|
||||||
extern uint32_t accTimeSum;
|
|
||||||
extern int accSumCount;
|
|
||||||
extern uint32_t currentTime;
|
|
||||||
extern uint16_t cycleTime;
|
|
||||||
extern uint16_t calibratingA;
|
|
||||||
extern uint16_t calibratingB;
|
|
||||||
extern uint16_t calibratingG;
|
|
||||||
extern int16_t heading;
|
|
||||||
extern int32_t baroPressure;
|
|
||||||
extern int32_t baroTemperature;
|
|
||||||
extern uint32_t baroPressureSum;
|
|
||||||
extern int32_t BaroAlt;
|
|
||||||
extern int32_t sonarAlt;
|
|
||||||
extern int32_t EstAlt;
|
|
||||||
extern int32_t AltHold;
|
|
||||||
extern int32_t errorAltitudeI;
|
|
||||||
extern int32_t BaroPID;
|
|
||||||
extern int32_t vario;
|
|
||||||
extern int16_t throttleAngleCorrection;
|
|
||||||
extern int16_t headFreeModeHold;
|
|
||||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
|
||||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
|
||||||
extern uint16_t rssi; // range: [0;1023]
|
|
||||||
extern int16_t telemTemperature1; // gyro sensor temperature
|
|
||||||
extern uint8_t toggleBeep;
|
|
||||||
|
|
||||||
extern flags_t f;
|
|
||||||
|
|
||||||
// IMU
|
|
||||||
void annexCode(void);
|
|
||||||
void computeIMU(void);
|
|
||||||
int getEstimatedAltitude(void);
|
|
||||||
|
|
||||||
// Output
|
|
||||||
void mixerResetMotors(void);
|
|
||||||
void writeServos(void);
|
|
||||||
void writeMotors(void);
|
|
||||||
void writeAllMotors(int16_t mc);
|
|
||||||
void mixTable(void);
|
|
||||||
|
|
||||||
// buzzer
|
|
||||||
void buzzer(bool warn_vbat);
|
|
||||||
void systemBeep(bool onoff);
|
|
||||||
|
|
||||||
// cli
|
|
||||||
void cliProcess(void);
|
|
|
@ -65,6 +65,20 @@ void serialRxInit(rxConfig_t *rxConfig, failsafe_t *failsafe)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
|
||||||
|
{
|
||||||
|
switch (rxConfig->serialrx_type) {
|
||||||
|
case SERIALRX_SPEKTRUM1024:
|
||||||
|
case SERIALRX_SPEKTRUM2048:
|
||||||
|
return spektrumFrameComplete();
|
||||||
|
case SERIALRX_SBUS:
|
||||||
|
return sbusFrameComplete();
|
||||||
|
case SERIALRX_SUMD:
|
||||||
|
return sumdFrameComplete();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
{
|
{
|
||||||
uint8_t chan;
|
uint8_t chan;
|
||||||
|
|
|
@ -41,4 +41,4 @@ typedef uint16_t (* rcReadRawDataPtr)(rxConfig_t *rxConfig, rxRuntimeConfig_t *r
|
||||||
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||||
|
|
||||||
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
||||||
|
bool isSerialRxFrameComplete(rxConfig_t *rxConfig);
|
||||||
|
|
|
@ -13,8 +13,8 @@ typedef enum AccelSensors {
|
||||||
extern uint8_t accHardware;
|
extern uint8_t accHardware;
|
||||||
extern sensor_align_e accAlign;
|
extern sensor_align_e accAlign;
|
||||||
extern acc_t acc;
|
extern acc_t acc;
|
||||||
extern uint16_t calibratingA;
|
|
||||||
|
|
||||||
|
bool isAccelerationCalibrationComplete(void);
|
||||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
|
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
|
||||||
void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse);
|
void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse);
|
||||||
|
|
|
@ -11,4 +11,5 @@ typedef struct gyroConfig_s {
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||||
void gyroGetADC(void);
|
void gyroGetADC(void);
|
||||||
|
bool isGyroCalibrationComplete(void);
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue