Browse Source
moved source files around in preparation to adding makefile build way
moved source files around in preparation to adding makefile build way
added makefile (thx gke) added linker script (thx gke) moved startups into src directory as well no code changes. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@105 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61master
timecop
13 years ago
45 changed files with 229 additions and 14182 deletions
-
79Makefile
-
8161baseflight.uvgui.timecop
-
121mw-svn/EEPROM.cpp
-
120mw-svn/GPS.cpp
-
343mw-svn/IMU.cpp
-
1063mw-svn/LCD.cpp
-
54mw-svn/LED.cpp
-
784mw-svn/MultiWii.cpp
-
647mw-svn/Output.cpp
-
285mw-svn/RX.cpp
-
1207mw-svn/Sensors.cpp
-
486mw-svn/Serial.cpp
-
427mw-svn/config.h
-
484mw-svn/def.h
-
0src/baseflight_startups/startup_stm32f10x_md.s
-
0src/baseflight_startups/startup_stm32f10x_md_gcc.s
-
0src/board.h
-
0src/cli.c
-
0src/config.c
-
0src/drv_adc.c
-
0src/drv_adc.h
-
0src/drv_adxl345.c
-
0src/drv_adxl345.h
-
0src/drv_bmp085.c
-
0src/drv_bmp085.h
-
0src/drv_hmc5883l.c
-
0src/drv_hmc5883l.h
-
0src/drv_i2c.c
-
0src/drv_i2c.h
-
0src/drv_mpu3050.c
-
0src/drv_mpu3050.h
-
0src/drv_pwm.c
-
0src/drv_pwm.h
-
0src/drv_system.c
-
0src/drv_system.h
-
0src/drv_uart.c
-
0src/drv_uart.h
-
0src/imu.c
-
0src/main.c
-
0src/mixer.c
-
0src/mw.c
-
0src/mw.h
-
0src/sensors.c
-
0src/serial.c
-
150stm32_flash.ld
@ -0,0 +1,79 @@ |
|||
#Derived from Atollic True Studio Makefile by Prof. Greg Egan 2012
|
|||
|
|||
SHELL=cmd |
|||
|
|||
# System configuration - UNCOMMENT AS DESIRED
|
|||
|
|||
#Atollic TrueStudio
|
|||
#CC = "C:\Program Files (x86)\Atollic\TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0\ARMTools\bin\arm-atollic-eabi-gcc"
|
|||
# OBJCOPY NOT PERMITTED IN FREEBY!
|
|||
#OBJCOPY = "C:\Program Files (x86)\Atollic\TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0\ARMTools\bin\arm-atollic-eabi-objcopy"
|
|||
#OPT = -Os
|
|||
|
|||
#Yagarto currently gcc 4.6.2
|
|||
#CC = "C:\Program Files (x86)\yagarto\bin\arm-none-eabi-gcc"
|
|||
#OBJCOPY = "C:\Program Files (x86)\yagarto\bin\arm-none-eabi-objcopy"
|
|||
#OPT = -Os
|
|||
|
|||
#Code Sourcery current gcc 4.6.1
|
|||
CC = arm-none-eabi-gcc |
|||
OPT = -Os |
|||
OBJCOPY = arm-none-eabi-objcopy |
|||
|
|||
RM = rm -rf |
|||
|
|||
# Define output directory
|
|||
OBJECT_DIR = obj |
|||
BIN_DIR = $(OBJECT_DIR) |
|||
LINK_SCRIPT="stm32_flash.ld" |
|||
|
|||
# Assembler, Compiler and Linker flags and linker script settings
|
|||
LINKER_FLAGS=-lm -mthumb -mcpu=cortex-m3 -Wl,--gc-sections -T$(LINK_SCRIPT) -static -Wl,-cref "-Wl,-Map=$(BIN_DIR)/baseflight.map" -Wl,--defsym=malloc_getpagesize_P=0x1000 |
|||
ASSEMBLER_FLAGS=-c $(OPT) -mcpu=cortex-m3 -mthumb -x assembler-with-cpp -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS\CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST\STM32F10x |
|||
COMPILER_FLAGS=-c -mcpu=cortex-m3 $(OPT) -Wall -ffunction-sections -fdata-sections -mthumb -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS\CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST\STM32F10x |
|||
|
|||
# Define sources and objects
|
|||
SRC := $(wildcard */*/*/*/*/*/*/*.c) \
|
|||
$(wildcard */*/*/*/*/*/*.c) \
|
|||
$(wildcard */*/*/*/*/*.c) \
|
|||
$(wildcard */*/*/*/*.c) \
|
|||
$(wildcard */*/*/*.c) \
|
|||
$(wildcard */*/*.c) \
|
|||
$(wildcard */*.c) |
|||
SRCSASM := $(wildcard */*/startup_stm32f10x_md_gcc.s) \
|
|||
#SRCSASM := $(wildcard */*/*/*/*/*/*/*/*.s) \
|
|||
# $(wildcard */*/*/*/*/*/*/*.s) \
|
|||
# $(wildcard */*/*/*/*/*/*.s) \
|
|||
# $(wildcard */*/*/*/*/*.s) \
|
|||
# $(wildcard */*/*/*/*.s) \
|
|||
# $(wildcard */*/*/*.s) \
|
|||
# $(wildcard */*/*.s) \
|
|||
# $(wildcard */*.s)
|
|||
OBJS := $(SRC:%.c=$(OBJECT_DIR)/%.o) $(SRCSASM:%.s=$(OBJECT_DIR)/%.o) |
|||
OBJS := $(OBJS:%.S=$(OBJECT_DIR)/%.o) |
|||
|
|||
all: buildelf |
|||
$(OBJCOPY) -O ihex "$(BIN_DIR)/baseflight.elf" "$(BIN_DIR)/baseflight.hex" |
|||
|
|||
buildelf: $(OBJS) |
|||
$(CC) -o "$(BIN_DIR)/baseflight.elf" $(OBJS) $(LINKER_FLAGS) |
|||
|
|||
clean: |
|||
$(RM) $(OBJS) "$(BIN_DIR)/*.*" |
|||
# $(RM) $(OBJS) "$(BIN_DIR)/baseflight.elf" "$(BIN_DIR)/baseflight.map" "$(BIN_DIR)/src/*.*" "$(BIN_DIR)/lib/*.*"
|
|||
|
|||
$(OBJECT_DIR)/main.o: main.c |
|||
@mkdir $(subst /,\,$(dir $@)) 2> NUL || echo off |
|||
$(CC) $(COMPILER_FLAGS) main.c -o $(OBJECT_DIR)/main.o |
|||
|
|||
$(OBJECT_DIR)/%.o: %.c |
|||
@mkdir $(subst /,\,$(dir $@)) 2> NUL || echo off |
|||
$(CC) $(COMPILER_FLAGS) $< -o $@ |
|||
|
|||
$(OBJECT_DIR)/%.o: %.s |
|||
@mkdir $(subst /,\,$(dir $@)) 2> NUL || echo off |
|||
$(CC) $(ASSEMBLER_FLAGS) $< -o $@ |
|||
|
|||
$(OBJECT_DIR)/%.o: %.S |
|||
@mkdir $(subst /,\,$(dir $@)) 2> NUL || echo off |
|||
$(CC) $(ASSEMBLER_FLAGS) $< -o $@ |
|||
8161
baseflight.uvgui.timecop
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
@ -1,121 +0,0 @@ |
|||
#include <avr/eeprom.h>
|
|||
|
|||
static uint8_t checkNewConf = 150; |
|||
|
|||
struct eep_entry_t { |
|||
void *var; |
|||
uint8_t size; |
|||
}; |
|||
|
|||
// ************************************************************************************************************
|
|||
// EEPROM Layout definition
|
|||
// ************************************************************************************************************
|
|||
static eep_entry_t eep_entry[] = { |
|||
{&checkNewConf, sizeof(checkNewConf)} |
|||
, {&P8, sizeof(P8)} |
|||
, {&I8, sizeof(I8)} |
|||
, {&D8, sizeof(D8)} |
|||
, {&rcRate8, sizeof(rcRate8)} |
|||
, {&rcExpo8, sizeof(rcExpo8)} |
|||
, {&rollPitchRate, sizeof(rollPitchRate)} |
|||
, {&yawRate, sizeof(yawRate)} |
|||
, {&dynThrPID, sizeof(dynThrPID)} |
|||
, {&accZero, sizeof(accZero)} |
|||
, {&magZero, sizeof(magZero)} |
|||
, {&accTrim, sizeof(accTrim)} |
|||
, {&activate1, sizeof(activate1)} |
|||
, {&activate2, sizeof(activate2)} |
|||
, {&powerTrigger1, sizeof(powerTrigger1)} |
|||
#ifdef FLYING_WING
|
|||
, {&wing_left_mid, sizeof(wing_left_mid)} |
|||
, {&wing_right_mid, sizeof(wing_right_mid)} |
|||
#endif
|
|||
#ifdef TRI
|
|||
, {&tri_yaw_middle, sizeof(tri_yaw_middle)} |
|||
#endif
|
|||
|
|||
}; |
|||
#define EEBLOCK_SIZE sizeof(eep_entry)/sizeof(eep_entry_t)
|
|||
// ************************************************************************************************************
|
|||
|
|||
void readEEPROM() |
|||
{ |
|||
uint8_t i, _address = eep_entry[0].size; |
|||
for (i = 1; i < EEBLOCK_SIZE; i++) { |
|||
eeprom_read_block(eep_entry[i].var, (void *) (_address), eep_entry[i].size); |
|||
_address += eep_entry[i].size; |
|||
} |
|||
#if defined(POWERMETER)
|
|||
pAlarm = (uint32_t) powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
|
|||
#endif
|
|||
for (i = 0; i < 7; i++) |
|||
lookupRX[i] = (2500 + rcExpo8 * (i * i - 25)) * i * (int32_t) rcRate8 / 1250; |
|||
#ifdef FLYING_WING
|
|||
wing_left_mid = constrain(wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
|||
wing_right_mid = constrain(wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
|||
#endif
|
|||
#ifdef TRI
|
|||
tri_yaw_middle = constrain(tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
|||
#endif
|
|||
} |
|||
|
|||
void writeParams() |
|||
{ |
|||
uint8_t i, _address = 0; |
|||
for (i = 0; i < EEBLOCK_SIZE; i++) { |
|||
eeprom_write_block(eep_entry[i].var, (void *) (_address), eep_entry[i].size); |
|||
_address += eep_entry[i].size; |
|||
} |
|||
readEEPROM(); |
|||
blinkLED(15, 20, 1); |
|||
} |
|||
|
|||
void checkFirstTime() |
|||
{ |
|||
uint8_t test_val; |
|||
eeprom_read_block((void *) &test_val, (void *) (0), sizeof(test_val)); |
|||
if (test_val == checkNewConf) |
|||
return; |
|||
P8[ROLL] = 40; |
|||
I8[ROLL] = 30; |
|||
D8[ROLL] = 23; |
|||
P8[PITCH] = 40; |
|||
I8[PITCH] = 30; |
|||
D8[PITCH] = 23; |
|||
P8[YAW] = 85; |
|||
I8[YAW] = 0; |
|||
D8[YAW] = 0; |
|||
P8[PIDALT] = 47; |
|||
I8[PIDALT] = 0; |
|||
D8[PIDALT] = 30; |
|||
P8[PIDGPS] = 10; |
|||
I8[PIDGPS] = 0; |
|||
D8[PIDGPS] = 0; |
|||
P8[PIDVEL] = 0; |
|||
I8[PIDVEL] = 0; |
|||
D8[PIDVEL] = 0; |
|||
P8[PIDLEVEL] = 90; |
|||
I8[PIDLEVEL] = 45; |
|||
D8[PIDLEVEL] = 100; |
|||
P8[PIDMAG] = 40; |
|||
rcRate8 = 45; // = 0.9 in GUI
|
|||
rcExpo8 = 65; |
|||
rollPitchRate = 0; |
|||
yawRate = 0; |
|||
dynThrPID = 0; |
|||
for (uint8_t i = 0; i < CHECKBOXITEMS; i++) { |
|||
activate1[i] = 0; |
|||
activate2[i] = 0; |
|||
} |
|||
accTrim[0] = 0; |
|||
accTrim[1] = 0; |
|||
powerTrigger1 = 0; |
|||
#ifdef FLYING_WING
|
|||
wing_left_mid = WING_LEFT_MID; |
|||
wing_right_mid = WING_RIGHT_MID; |
|||
#endif
|
|||
#ifdef TRI
|
|||
tri_yaw_middle = TRI_YAW_MIDDLE; |
|||
#endif
|
|||
writeParams(); |
|||
} |
|||
@ -1,120 +0,0 @@ |
|||
/* this is an equirectangular approximation to calculate distance and bearing between 2 GPS points (lat/long)
|
|||
it's much more faster than an exact calculation |
|||
the error is neglectible for few kilometers assuming a constant R for earth |
|||
input: lat1/long1 <-> lat2/long2 unit: 1/100000 degree |
|||
output: distance in meters, bearing in degrees |
|||
*/ |
|||
void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing) |
|||
{ |
|||
float dLat = ((lat2 - lat1)); // difference of latitude in 1/100000 degrees
|
|||
float dLon = ((lon2 - lon1)) * cos(lat1 * (PI / 180 / 100000.0)); // difference of longitude in 1/100000 degrees
|
|||
*dist = 6372795 / 100000.0 * PI / 180 * (sqrt(sq(dLat) + sq(dLon))); |
|||
if (lat1 != lat2) |
|||
*bearing = 180 / PI * (atan2(dLon, dLat)); |
|||
else |
|||
*bearing = 0; |
|||
} |
|||
|
|||
/* The latitude or longitude is coded this way in NMEA frames
|
|||
dm.m coded as degrees + minutes + minute decimal |
|||
Where: |
|||
- d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude |
|||
- m is always 2 char long |
|||
- m can be 1 or more char long |
|||
This function converts this format in a unique unsigned long where 1 degree = 100 000 |
|||
*/ |
|||
uint32_t GPS_coord_to_degrees(char *s) |
|||
{ |
|||
char *p, *d = s; |
|||
uint32_t sec, m = 1000; |
|||
uint16_t min, dec = 0; |
|||
|
|||
if (!*s) |
|||
return 0; |
|||
for (p = s; *p != 0; p++) { |
|||
if (d != s) { |
|||
*p -= '0'; |
|||
dec += *p * m; |
|||
m /= 10; |
|||
} |
|||
if (*p == '.') |
|||
d = p; |
|||
} |
|||
m = 10000; |
|||
min = *--d - '0' + (*--d - '0') * 10; |
|||
sec = (m * min + dec) / 6; |
|||
while (d != s) { |
|||
m *= 10; |
|||
*--d -= '0'; |
|||
sec += *d * m; |
|||
} |
|||
return sec; |
|||
} |
|||
|
|||
|
|||
/* This is a light implementation of a GPS frame decoding
|
|||
This should work with most of modern GPS devices configured to output NMEA frames. |
|||
It assumes there are some NMEA GGA frames to decode on the serial bus |
|||
Here we use only the following data : |
|||
- latitude |
|||
- longitude |
|||
- GPS fix is/is not ok |
|||
- GPS num sat (4 is enough to be +/- reliable) |
|||
*/ |
|||
bool GPS_newFrame(char c) |
|||
{ |
|||
uint8_t frameOK = 0; |
|||
static uint8_t param = 0, offset = 0, parity = 0; |
|||
static char string[15]; |
|||
static uint8_t checksum_param, GPGGA_frame = 0; |
|||
|
|||
if (c == '$') { |
|||
param = 0; |
|||
offset = 0; |
|||
parity = 0; |
|||
} else if (c == ',' || c == '*') { |
|||
string[offset] = 0; |
|||
if (param == 0) { //frame identification
|
|||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') |
|||
GPGGA_frame = 1; |
|||
else |
|||
GPGGA_frame = 0; |
|||
} else if (GPGGA_frame == 1) { |
|||
if (param == 2) { |
|||
GPS_latitude = GPS_coord_to_degrees(string); |
|||
} else if (param == 3 && string[0] == 'S') |
|||
GPS_latitude = -GPS_latitude; |
|||
else if (param == 4) { |
|||
GPS_longitude = GPS_coord_to_degrees(string); |
|||
} else if (param == 5 && string[0] == 'W') |
|||
GPS_longitude = -GPS_longitude; |
|||
else if (param == 6) { |
|||
GPS_fix = string[0] > '0'; |
|||
} else if (param == 7) { |
|||
if (offset > 1) |
|||
GPS_numSat = (string[0] - '0') * 10 + string[1] - '0'; |
|||
else |
|||
GPS_numSat = string[0] - '0'; |
|||
} |
|||
} |
|||
param++; |
|||
offset = 0; |
|||
if (c == '*') |
|||
checksum_param = 1; |
|||
else |
|||
parity ^= c; |
|||
} else if (c == '\r' || c == '\n') { |
|||
if (checksum_param) { //parity checksum
|
|||
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); |
|||
if (checksum == parity) |
|||
frameOK = 1; |
|||
} |
|||
checksum_param = 0; |
|||
} else { |
|||
if (offset < 15) |
|||
string[offset++] = c; |
|||
if (!checksum_param) |
|||
parity ^= c; |
|||
} |
|||
return frameOK && GPGGA_frame; |
|||
} |
|||
@ -1,343 +0,0 @@ |
|||
|
|||
void computeIMU() |
|||
{ |
|||
uint8_t axis; |
|||
static int16_t gyroADCprevious[3] = { 0, 0, 0 }; |
|||
int16_t gyroADCp[3]; |
|||
int16_t gyroADCinter[3]; |
|||
static uint32_t timeInterleave = 0; |
|||
#if defined(TRI)
|
|||
static int16_t gyroYawSmooth = 0; |
|||
#endif
|
|||
|
|||
if (MAG) |
|||
Mag_getADC(); |
|||
if (BARO) |
|||
Baro_update(); |
|||
|
|||
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
|
|||
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
|
|||
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
|
|||
if (!ACC && nunchuk) { |
|||
annexCode(); |
|||
while ((micros() - timeInterleave) < INTERLEAVING_DELAY); //interleaving delay between 2 consecutive reads
|
|||
timeInterleave = micros(); |
|||
WMP_getRawADC(); |
|||
getEstimatedAttitude(); // computation time must last less than one interleaving delay
|
|||
#if BARO
|
|||
getEstimatedAltitude(); |
|||
#endif
|
|||
while ((micros() - timeInterleave) < INTERLEAVING_DELAY); //interleaving delay between 2 consecutive reads
|
|||
timeInterleave = micros(); |
|||
while (WMP_getRawADC() != 1); // For this interleaving reading, we must have a gyro update at this point (less delay)
|
|||
|
|||
for (axis = 0; axis < 3; axis++) { |
|||
// empirical, we take a weighted value of the current and the previous values
|
|||
// /4 is to average 4 values, note: overflow is not possible for WMP gyro here
|
|||
gyroData[axis] = (gyroADC[axis] * 3 + gyroADCprevious[axis] + 2) / 4; |
|||
gyroADCprevious[axis] = gyroADC[axis]; |
|||
} |
|||
} else { |
|||
if (ACC) { |
|||
ACC_getADC(); |
|||
getEstimatedAttitude(); |
|||
if (BARO) |
|||
getEstimatedAltitude(); |
|||
} |
|||
if (GYRO) |
|||
Gyro_getADC(); |
|||
else |
|||
WMP_getRawADC(); |
|||
for (axis = 0; axis < 3; axis++) |
|||
gyroADCp[axis] = gyroADC[axis]; |
|||
timeInterleave = micros(); |
|||
annexCode(); |
|||
if ((micros() - timeInterleave) > 650) { |
|||
annex650_overrun_count++; |
|||
} else { |
|||
while ((micros() - timeInterleave) < 650); //empirical, interleaving delay between 2 consecutive reads
|
|||
} |
|||
if (GYRO) |
|||
Gyro_getADC(); |
|||
else |
|||
WMP_getRawADC(); |
|||
for (axis = 0; axis < 3; axis++) { |
|||
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis]; |
|||
// empirical, we take a weighted value of the current and the previous values
|
|||
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis] + 1) / 3; |
|||
gyroADCprevious[axis] = gyroADCinter[axis] / 2; |
|||
if (!ACC) |
|||
accADC[axis] = 0; |
|||
} |
|||
} |
|||
#if defined(TRI)
|
|||
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3; |
|||
gyroYawSmooth = gyroData[YAW]; |
|||
#endif
|
|||
} |
|||
|
|||
// **************************************************
|
|||
// Simplified IMU based on "Complementary Filter"
|
|||
// Inspired by http://starlino.com/imu_guide.html
|
|||
//
|
|||
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
|
|||
//
|
|||
// The following ideas was used in this project:
|
|||
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
|
|||
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
|
|||
// 3) C. Hastings approximation for atan2()
|
|||
// 4) Optimization tricks: http://www.hackersdelight.org/
|
|||
//
|
|||
// Currently Magnetometer uses separate CF which is used only
|
|||
// for heading approximation.
|
|||
//
|
|||
// Modified: 19/04/2011 by ziss_dm
|
|||
// Version: V1.1
|
|||
//
|
|||
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
|
|||
// **************************************************
|
|||
|
|||
//****** advanced users settings *******************
|
|||
/* Set the Low Pass Filter factor for ACC */ |
|||
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/ |
|||
/* Comment this if you do not want filter at all.*/ |
|||
/* Default WMC value: 8*/ |
|||
#define ACC_LPF_FACTOR 4
|
|||
|
|||
/* Set the Low Pass Filter factor for Magnetometer */ |
|||
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/ |
|||
/* Comment this if you do not want filter at all.*/ |
|||
/* Default WMC value: n/a*/ |
|||
//#define MG_LPF_FACTOR 4
|
|||
|
|||
/* Set the Gyro Weight for Gyro/Acc complementary filter */ |
|||
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/ |
|||
/* Default WMC value: 300*/ |
|||
#define GYR_CMPF_FACTOR 310.0f
|
|||
|
|||
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */ |
|||
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/ |
|||
/* Default WMC value: n/a*/ |
|||
#define GYR_CMPFM_FACTOR 200.0f
|
|||
|
|||
//****** end of advanced users settings *************
|
|||
|
|||
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
|
|||
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
|
|||
#if GYRO
|
|||
#define GYRO_SCALE ((2380 * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
|
|||
// +-2000/sec deg scale
|
|||
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
|
|||
// +- 200/sec deg scale
|
|||
// 1.5 is emperical, not sure what it means
|
|||
// should be in rad/sec
|
|||
#else
|
|||
#define GYRO_SCALE (1.0f/200e6f)
|
|||
// empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
|
|||
// !!!!should be adjusted to the rad/sec
|
|||
#endif
|
|||
// Small angle approximation
|
|||
#define ssin(val) (val)
|
|||
#define scos(val) 1.0f
|
|||
|
|||
typedef struct fp_vector { |
|||
float X; |
|||
float Y; |
|||
float Z; |
|||
} t_fp_vector_def; |
|||
|
|||
typedef union { |
|||
float A[3]; |
|||
t_fp_vector_def V; |
|||
} t_fp_vector; |
|||
|
|||
int16_t _atan2(float y, float x) |
|||
{ |
|||
#define fp_is_neg(val) ((((byte*)&val)[3] & 0x80) != 0)
|
|||
float z = y / x; |
|||
int16_t zi = abs(int16_t(z * 100)); |
|||
int8_t y_neg = fp_is_neg(y); |
|||
if (zi < 100) { |
|||
if (zi > 10) |
|||
z = z / (1.0f + 0.28f * z * z); |
|||
if (fp_is_neg(x)) { |
|||
if (y_neg) |
|||
z -= PI; |
|||
else |
|||
z += PI; |
|||
} |
|||
} else { |
|||
z = (PI / 2.0f) - z / (z * z + 0.28f); |
|||
if (y_neg) |
|||
z -= PI; |
|||
} |
|||
z *= (180.0f / PI * 10); |
|||
return z; |
|||
} |
|||
|
|||
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
|
|||
void rotateV(struct fp_vector *v, float *delta) |
|||
{ |
|||
fp_vector v_tmp = *v; |
|||
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y; |
|||
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y; |
|||
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X; |
|||
} |
|||
|
|||
void getEstimatedAttitude() |
|||
{ |
|||
uint8_t axis; |
|||
int32_t accMag = 0; |
|||
static t_fp_vector EstG; |
|||
#if MAG
|
|||
static t_fp_vector EstM; |
|||
#endif
|
|||
#if defined(MG_LPF_FACTOR)
|
|||
static int16_t mgSmooth[3]; |
|||
#endif
|
|||
#if defined(ACC_LPF_FACTOR)
|
|||
static int16_t accTemp[3]; //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
|
|||
#endif
|
|||
static uint16_t previousT; |
|||
uint16_t currentT = micros(); |
|||
float scale, deltaGyroAngle[3]; |
|||
|
|||
scale = (currentT - previousT) * GYRO_SCALE; |
|||
previousT = currentT; |
|||
|
|||
// Initialization
|
|||
for (axis = 0; axis < 3; axis++) { |
|||
deltaGyroAngle[axis] = gyroADC[axis] * scale; |
|||
#if defined(ACC_LPF_FACTOR)
|
|||
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> ACC_LPF_FACTOR)) + accADC[axis]; |
|||
accSmooth[axis] = accTemp[axis] >> ACC_LPF_FACTOR; |
|||
#define ACC_VALUE accSmooth[axis]
|
|||
#else
|
|||
accSmooth[axis] = accADC[axis]; |
|||
#define ACC_VALUE accADC[axis]
|
|||
#endif
|
|||
// accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
|
|||
accMag += (int32_t) ACC_VALUE *ACC_VALUE; |
|||
#if MAG
|
|||
#if defined(MG_LPF_FACTOR)
|
|||
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
|
|||
#define MAG_VALUE mgSmooth[axis]
|
|||
#else
|
|||
#define MAG_VALUE magADC[axis]
|
|||
#endif
|
|||
#endif
|
|||
} |
|||
accMag = accMag * 100 / ((int32_t) acc_1G * acc_1G); |
|||
|
|||
rotateV(&EstG.V, deltaGyroAngle); |
|||
#if MAG
|
|||
rotateV(&EstM.V, deltaGyroAngle); |
|||
#endif
|
|||
|
|||
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0) |
|||
smallAngle25 = 1; |
|||
else |
|||
smallAngle25 = 0; |
|||
|
|||
// Apply complimentary filter (Gyro drift correction)
|
|||
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
|
|||
// To do that, we just skip filter, as EstV already rotated by Gyro
|
|||
if ((36 < accMag && accMag < 196) || smallAngle25) |
|||
for (axis = 0; axis < 3; axis++) { |
|||
int16_t acc = ACC_VALUE; |
|||
#if not defined(TRUSTED_ACCZ)
|
|||
if (smallAngle25 && axis == YAW) |
|||
//We consider ACCZ = acc_1G when the acc on other axis is small.
|
|||
//It's a tweak to deal with some configs where ACC_Z tends to a value < acc_1G when high throttle is applied.
|
|||
//This tweak applies only when the multi is not in inverted position
|
|||
acc = acc_1G; |
|||
#endif
|
|||
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR; |
|||
} |
|||
#if MAG
|
|||
for (axis = 0; axis < 3; axis++) |
|||
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR; |
|||
#endif
|
|||
|
|||
// Attitude of the estimated vector
|
|||
angle[ROLL] = _atan2(EstG.V.X, EstG.V.Z); |
|||
angle[PITCH] = _atan2(EstG.V.Y, EstG.V.Z); |
|||
#if MAG
|
|||
// Attitude of the cross product vector GxM
|
|||
heading = _atan2(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10; |
|||
#endif
|
|||
} |
|||
|
|||
float InvSqrt(float x) |
|||
{ |
|||
union { |
|||
int32_t i; |
|||
float f; |
|||
} conv; |
|||
conv.f = x; |
|||
conv.i = 0x5f3759df - (conv.i >> 1); |
|||
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f); |
|||
} |
|||
|
|||
int32_t isq(int32_t x) |
|||
{ |
|||
return x * x; |
|||
} |
|||
|
|||
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
|||
#define INIT_DELAY 4000000 // 4 sec initialization delay
|
|||
#define Kp1 5.5f // PI observer velocity gain
|
|||
#define Kp2 10.0f // PI observer position gain
|
|||
#define Ki 0.01f // PI observer integral gain (bias cancellation)
|
|||
#define dt (UPDATE_INTERVAL / 1000000.0f)
|
|||
|
|||
void getEstimatedAltitude() |
|||
{ |
|||
static uint8_t inited = 0; |
|||
static int16_t AltErrorI = 0; |
|||
static float AccScale; |
|||
static uint32_t deadLine = INIT_DELAY; |
|||
int16_t AltError; |
|||
int16_t InstAcc; |
|||
static int32_t tmpAlt; |
|||
static int16_t EstVelocity = 0; |
|||
static uint32_t velTimer; |
|||
static int16_t lastAlt; |
|||
|
|||
if (currentTime < deadLine) |
|||
return; |
|||
deadLine = currentTime + UPDATE_INTERVAL; |
|||
// Soft start
|
|||
|
|||
if (!inited) { |
|||
inited = 1; |
|||
tmpAlt = BaroAlt * 10; |
|||
AccScale = 100 * 9.80665f / acc_1G; |
|||
} |
|||
// Estimation Error
|
|||
AltError = BaroAlt - EstAlt; |
|||
AltErrorI += AltError; |
|||
AltErrorI = constrain(AltErrorI, -2500, +2500); |
|||
// Gravity vector correction and projection to the local Z
|
|||
//InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
|
|||
#if defined(TRUSTED_ACCZ)
|
|||
InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + AltErrorI / 100; |
|||
#else
|
|||
InstAcc = AltErrorI / 100; |
|||
#endif
|
|||
|
|||
// Integrators
|
|||
tmpAlt += EstVelocity * (dt * dt) + (Kp2 * dt) * AltError; |
|||
EstVelocity += InstAcc + Kp1 * AltError; |
|||
EstVelocity = constrain(EstVelocity, -10000, +10000); |
|||
|
|||
EstAlt = tmpAlt / 10; |
|||
|
|||
if (currentTime < velTimer) |
|||
return; |
|||
velTimer = currentTime + 500000; |
|||
zVelocity = tmpAlt - lastAlt; |
|||
lastAlt = tmpAlt; |
|||
|
|||
debug4 = zVelocity; |
|||
} |
|||
1063
mw-svn/LCD.cpp
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
@ -1,54 +0,0 @@ |
|||
#if defined(LED_RING)
|
|||
|
|||
#define LED_RING_ADDRESS 0xDA //8 bits -- my initial :)
|
|||
|
|||
void i2CLedRingState() |
|||
{ |
|||
uint8_t b[10]; |
|||
static uint8_t state; |
|||
|
|||
if (state == 0) { |
|||
b[0] = 'z'; |
|||
b[1] = (180 - heading) / 2; // 1 unit = 2 degrees;
|
|||
i2c_rep_start(LED_RING_ADDRESS); |
|||
for (uint8_t i = 0; i < 2; i++) |
|||
i2c_write(b[i]); |
|||
i2c_stop(); |
|||
state = 1; |
|||
} else if (state == 1) { |
|||
b[0] = 'y'; |
|||
b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); |
|||
b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); |
|||
i2c_rep_start(LED_RING_ADDRESS); |
|||
for (uint8_t i = 0; i < 3; i++) |
|||
i2c_write(b[i]); |
|||
i2c_stop(); |
|||
state = 2; |
|||
} else if (state == 2) { |
|||
b[0] = 'd'; // all unicolor GREEN
|
|||
b[1] = 1; |
|||
if (armed) |
|||
b[2] = 1; |
|||
else |
|||
b[2] = 0; |
|||
i2c_rep_start(LED_RING_ADDRESS); |
|||
for (uint8_t i = 0; i < 3; i++) |
|||
i2c_write(b[i]); |
|||
i2c_stop(); |
|||
state = 0; |
|||
} |
|||
} |
|||
|
|||
void blinkLedRing() |
|||
{ |
|||
uint8_t b[3]; |
|||
b[0] = 'k'; |
|||
b[1] = 10; |
|||
b[2] = 10; |
|||
i2c_rep_start(LED_RING_ADDRESS); |
|||
for (uint8_t i = 0; i < 3; i++) |
|||
i2c_write(b[i]); |
|||
i2c_stop(); |
|||
} |
|||
|
|||
#endif
|
|||
@ -1,784 +0,0 @@ |
|||
/*
|
|||
MultiWiiCopter by Alexandre Dubus |
|||
www.multiwii.com |
|||
December 2011 V1.dev |
|||
This program is free software: you can redistribute it and/or modify |
|||
it under the terms of the GNU General Public License as published by |
|||
the Free Software Foundation, either version 3 of the License, or |
|||
any later version. see <http://www.gnu.org/licenses/>
|
|||
*/ |
|||
|
|||
#include "config.h"
|
|||
#include "def.h"
|
|||
#include <avr/pgmspace.h>
|
|||
#define VERSION 19
|
|||
|
|||
/*********** RC alias *****************/ |
|||
#define ROLL 0
|
|||
#define PITCH 1
|
|||
#define YAW 2
|
|||
#define THROTTLE 3
|
|||
#define AUX1 4
|
|||
#define AUX2 5
|
|||
#define AUX3 6
|
|||
#define AUX4 7
|
|||
|
|||
#define PIDALT 3
|
|||
#define PIDVEL 4
|
|||
#define PIDGPS 5
|
|||
#define PIDLEVEL 6
|
|||
#define PIDMAG 7
|
|||
|
|||
#define BOXACC 0
|
|||
#define BOXBARO 1
|
|||
#define BOXMAG 2
|
|||
#define BOXCAMSTAB 3
|
|||
#define BOXCAMTRIG 4
|
|||
#define BOXARM 5
|
|||
#define BOXGPSHOME 6
|
|||
#define BOXGPSHOLD 7
|
|||
#define BOXPASSTHRU 8
|
|||
#define BOXHEADFREE 9
|
|||
#define BOXBEEPERON 10
|
|||
|
|||
#define CHECKBOXITEMS 11
|
|||
#define PIDITEMS 8
|
|||
|
|||
static uint32_t currentTime = 0; |
|||
static uint16_t previousTime = 0; |
|||
static 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
|
|||
static 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.
|
|||
static uint8_t calibratingM = 0; |
|||
static uint16_t calibratingG; |
|||
static uint8_t armed = 0; |
|||
static uint16_t acc_1G; // this is the 1G measured acceleration
|
|||
static int16_t acc_25deg; |
|||
static uint8_t nunchuk = 0; |
|||
static uint8_t accMode = 0; // if level mode is a activated
|
|||
static uint8_t magMode = 0; // if compass heading hold is a activated
|
|||
static uint8_t baroMode = 0; // if altitude hold is activated
|
|||
static uint8_t GPSModeHome = 0; // if GPS RTH is activated
|
|||
static uint8_t GPSModeHold = 0; // if GPS PH is activated
|
|||
static uint8_t headFreeMode = 0; // if head free mode is a activated
|
|||
static uint8_t passThruMode = 0; // if passthrough mode is activated
|
|||
static int16_t headFreeModeHold; |
|||
static int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; |
|||
static int16_t accTrim[2] = { 0, 0 }; |
|||
static int16_t heading, magHold; |
|||
static uint8_t calibratedACC = 0; |
|||
static uint8_t vbat; // battery voltage in 0.1V steps
|
|||
static uint8_t okToArm = 0; |
|||
static uint8_t rcOptions1, rcOptions2; |
|||
static int32_t pressure; |
|||
static int16_t BaroAlt; |
|||
static int16_t EstAlt; // in cm
|
|||
static int16_t zVelocity; |
|||
static uint8_t buzzerState = 0; |
|||
static int16_t debug1, debug2, debug3, debug4; |
|||
|
|||
//for log
|
|||
static uint16_t cycleTimeMax = 0; // highest ever cycle timen
|
|||
static uint16_t cycleTimeMin = 65535; // lowest ever cycle timen
|
|||
static uint16_t powerMax = 0; // highest ever current
|
|||
|
|||
static int16_t i2c_errors_count = 0; |
|||
static int16_t annex650_overrun_count = 0; |
|||
|
|||
// **********************
|
|||
//Automatic ACC Offset Calibration
|
|||
// **********************
|
|||
static uint16_t InflightcalibratingA = 0; |
|||
static int16_t AccInflightCalibrationArmed; |
|||
static uint16_t AccInflightCalibrationMeasurementDone = 0; |
|||
static uint16_t AccInflightCalibrationSavetoEEProm = 0; |
|||
|
|||
// **********************
|
|||
// power meter
|
|||
// **********************
|
|||
#define PMOTOR_SUM 8 // index into pMeter[] for sum
|
|||
static uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
|
|||
static uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
|||
static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
|||
static uint8_t powerTrigger1 = 0; // trigger for alarm based on power consumption
|
|||
static uint16_t powerValue = 0; // last known current
|
|||
static uint16_t intPowerMeterSum, intPowerTrigger1; |
|||
|
|||
// **********************
|
|||
// telemetry
|
|||
// **********************
|
|||
static uint8_t telemetry = 0; |
|||
static uint8_t telemetry_auto = 0; |
|||
|
|||
// ******************
|
|||
// rc functions
|
|||
// ******************
|
|||
#define MINCHECK 1100
|
|||
#define MAXCHECK 1900
|
|||
|
|||
volatile int16_t failsafeCnt = 0; |
|||
static int16_t failsafeEvents = 0; |
|||
static int16_t rcData[8]; // interval [1000;2000]
|
|||
static int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
|||
static uint8_t rcRate8; |
|||
static uint8_t rcExpo8; |
|||
static int16_t lookupRX[7]; // lookup table for expo & RC rate
|
|||
volatile uint8_t rcFrameComplete; //for serial rc receiver Spektrum
|
|||
|
|||
|
|||
// **************
|
|||
// gyro+acc IMU
|
|||
// **************
|
|||
static int16_t gyroData[3] = { 0, 0, 0 }; |
|||
static int16_t gyroZero[3] = { 0, 0, 0 }; |
|||
static int16_t accZero[3] = { 0, 0, 0 }; |
|||
static int16_t magZero[3] = { 0, 0, 0 }; |
|||
static int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
|||
static int8_t smallAngle25 = 1; |
|||
|
|||
// *************************
|
|||
// motor and servo functions
|
|||
// *************************
|
|||
static int16_t axisPID[3]; |
|||
static int16_t motor[8]; |
|||
static int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; |
|||
static uint16_t wing_left_mid = WING_LEFT_MID; |
|||
static uint16_t wing_right_mid = WING_RIGHT_MID; |
|||
static uint16_t tri_yaw_middle = TRI_YAW_MIDDLE; |
|||
|
|||
// **********************
|
|||
// EEPROM & LCD functions
|
|||
// **********************
|
|||
static uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
|
|||
static uint8_t dynP8[3], dynI8[3], dynD8[3]; |
|||
static uint8_t rollPitchRate; |
|||
static uint8_t yawRate; |
|||
static uint8_t dynThrPID; |
|||
static uint8_t activate1[CHECKBOXITEMS]; |
|||
static uint8_t activate2[CHECKBOXITEMS]; |
|||
|
|||
// **********************
|
|||
// GPS
|
|||
// **********************
|
|||
static int32_t GPS_latitude, GPS_longitude; |
|||
static int32_t GPS_latitude_home, GPS_longitude_home; |
|||
static uint8_t GPS_fix, GPS_fix_home = 0; |
|||
static uint8_t GPS_numSat; |
|||
static uint16_t GPS_distanceToHome; // in meters
|
|||
static int16_t GPS_directionToHome = 0; // in degrees
|
|||
static uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
|||
static int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
|||
|
|||
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat) |
|||
{ |
|||
uint8_t i, r; |
|||
for (r = 0; r < repeat; r++) { |
|||
for (i = 0; i < num; i++) { |
|||
LEDPIN_TOGGLE; //switch LEDPIN state
|
|||
BUZZERPIN_ON; |
|||
delay(wait); |
|||
BUZZERPIN_OFF; |
|||
} |
|||
delay(60); |
|||
} |
|||
} |
|||
|
|||
void annexCode() |
|||
{ //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
|
|||
static uint32_t buzzerTime, calibratedAccTime; |
|||
#if defined(LCD_TELEMETRY)
|
|||
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0; |
|||
#endif
|
|||
#if defined(LCD_TELEMETRY)
|
|||
static uint8_t telemetryAutoIndex = 0; |
|||
static char telemetryAutoSequence[] = LCD_TELEMETRY_AUTO; |
|||
#endif
|
|||
#ifdef VBAT
|
|||
static uint8_t vbatTimer = 0; |
|||
#endif
|
|||
static uint8_t buzzerFreq; //delay between buzzer ring
|
|||
uint8_t axis, prop1, prop2; |
|||
#if defined(POWERMETER_HARD)
|
|||
uint16_t pMeterRaw; //used for current reading
|
|||
#endif
|
|||
|
|||
//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
|||
if (rcData[THROTTLE] < 1500) { |
|||
prop2 = 100; |
|||
} else if (rcData[THROTTLE] < 2000) { |
|||
prop2 = 100 - (uint16_t) dynThrPID *(rcData[THROTTLE] - 1500) / 500; |
|||
} else { |
|||
prop2 = 100 - dynThrPID; |
|||
} |
|||
|
|||
for (axis = 0; axis < 3; axis++) { |
|||
uint16_t tmp = min(abs(rcData[axis] - MIDRC), 500); |
|||
#if defined(DEADBAND)
|
|||
if (tmp > DEADBAND) { |
|||
tmp -= DEADBAND; |
|||
} else { |
|||
tmp = 0; |
|||
} |
|||
#endif
|
|||
if (axis != 2) { //ROLL & PITCH
|
|||
uint16_t tmp2 = tmp / 100; |
|||
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100; |
|||
prop1 = 100 - (uint16_t) rollPitchRate *tmp / 500; |
|||
prop1 = (uint16_t) prop1 *prop2 / 100; |
|||
} else { //YAW
|
|||
rcCommand[axis] = tmp; |
|||
prop1 = 100 - (uint16_t) yawRate *tmp / 500; |
|||
} |
|||
dynP8[axis] = (uint16_t) P8[axis] * prop1 / 100; |
|||
dynD8[axis] = (uint16_t) D8[axis] * prop1 / 100; |
|||
if (rcData[axis] < MIDRC) |
|||
rcCommand[axis] = -rcCommand[axis]; |
|||
} |
|||
rcCommand[THROTTLE] = MINTHROTTLE + (int32_t) (MAXTHROTTLE - MINTHROTTLE) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK); |
|||
|
|||
if (headFreeMode) { |
|||
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
|
|||
float cosDiff = cos(radDiff); |
|||
float sinDiff = sin(radDiff); |
|||
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; |
|||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; |
|||
rcCommand[PITCH] = rcCommand_PITCH; |
|||
} |
|||
#if defined(POWERMETER_HARD)
|
|||
if (!(++psensorTimer % PSENSORFREQ)) { |
|||
pMeterRaw = analogRead(PSENSORPIN); |
|||
powerValue = (PSENSORNULL > pMeterRaw ? PSENSORNULL - pMeterRaw : pMeterRaw - PSENSORNULL); // do not use abs(), it would induce implicit cast to uint and overrun
|
|||
if (powerValue < 333) { // only accept reasonable values. 333 is empirical
|
|||
#ifdef LOG_VALUES
|
|||
if (powerValue > powerMax) |
|||
powerMax = powerValue; |
|||
#endif
|
|||
} else { |
|||
powerValue = 333; |
|||
} |
|||
pMeter[PMOTOR_SUM] += (uint32_t) powerValue; |
|||
} |
|||
#endif
|
|||
|
|||
#if defined(VBAT)
|
|||
static uint8_t ind = 0; |
|||
uint16_t vbatRaw = 0; |
|||
static uint16_t vbatRawArray[8]; |
|||
if (!(++vbatTimer % VBATFREQ)) { |
|||
ADCSRA |= _BV(ADPS2); |
|||
ADCSRA &= ~_BV(ADPS1); |
|||
ADCSRA &= ~_BV(ADPS0); //this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11
|
|||
vbatRawArray[(ind++) % 8] = analogRead(V_BATPIN); |
|||
for (uint8_t i = 0; i < 8; i++) |
|||
vbatRaw += vbatRawArray[i]; |
|||
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
|
|||
} |
|||
if ((rcOptions1 & activate1[BOXBEEPERON]) || (rcOptions2 & activate2[BOXBEEPERON])) { // unconditional beeper on via AUXn switch
|
|||
buzzerFreq = 7; |
|||
} else if (((vbat > VBATLEVEL1_3S) |
|||
#if defined(POWERMETER)
|
|||
&& ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0)) |
|||
#endif
|
|||
) || (NO_VBAT > vbat)) // ToLuSe
|
|||
{ //VBAT ok AND powermeter ok, buzzer off
|
|||
buzzerFreq = 0; |
|||
buzzerState = 0; |
|||
BUZZERPIN_OFF; |
|||
#if defined(POWERMETER)
|
|||
} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeter
|
|||
buzzerFreq = 4; |
|||
#endif
|
|||
} else if (vbat > VBATLEVEL2_3S) |
|||
buzzerFreq = 1; |
|||
else if (vbat > VBATLEVEL3_3S) |
|||
buzzerFreq = 2; |
|||
else |
|||
buzzerFreq = 4; |
|||
if (buzzerFreq) { |
|||
if (buzzerState && (currentTime > buzzerTime + 250000)) { |
|||
buzzerState = 0; |
|||
BUZZERPIN_OFF; |
|||
buzzerTime = currentTime; |
|||
} else if (!buzzerState && (currentTime > (buzzerTime + (2000000 >> buzzerFreq)))) { |
|||
buzzerState = 1; |
|||
BUZZERPIN_ON; |
|||
buzzerTime = currentTime; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
if ((calibratingA > 0 && (ACC || nunchuk)) || (calibratingG > 0)) { // Calibration phasis
|
|||
LEDPIN_TOGGLE; |
|||
} else { |
|||
if (calibratedACC == 1) { |
|||
LEDPIN_OFF; |
|||
} |
|||
if (armed) { |
|||
LEDPIN_ON; |
|||
} |
|||
} |
|||
|
|||
#if defined(LED_RING)
|
|||
static uint32_t LEDTime; |
|||
if (currentTime > LEDTime) { |
|||
LEDTime = currentTime + 50000; |
|||
i2CLedRingState(); |
|||
} |
|||
#endif
|
|||
|
|||
if (currentTime > calibratedAccTime) { |
|||
if (smallAngle25 == 0) { |
|||
calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much inclinated
|
|||
LEDPIN_TOGGLE; |
|||
calibratedAccTime = currentTime + 500000; |
|||
} else |
|||
calibratedACC = 1; |
|||
} |
|||
|
|||
serialCom(); |
|||
|
|||
#if defined(POWERMETER)
|
|||
intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV); |
|||
intPowerTrigger1 = powerTrigger1 * PLEVELSCALE; |
|||
#endif
|
|||
|
|||
#ifdef LCD_TELEMETRY_AUTO
|
|||
if ((telemetry_auto) |
|||
&& (!(++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ))) { |
|||
telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)]; |
|||
LCDclear(); // make sure to clear away remnants
|
|||
} |
|||
#endif
|
|||
#ifdef LCD_TELEMETRY
|
|||
if (!(++telemetryTimer % LCD_TELEMETRY_FREQ)) { |
|||
#if (LCD_TELEMETRY_DEBUG+0 > 0)
|
|||
telemetry = LCD_TELEMETRY_DEBUG; |
|||
#endif
|
|||
if (telemetry) |
|||
lcd_telemetry(); |
|||
} |
|||
#endif
|
|||
|
|||
#if defined(GPS)
|
|||
static uint32_t GPSLEDTime; |
|||
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) { |
|||
GPSLEDTime = currentTime + 150000; |
|||
LEDPIN_TOGGLE; |
|||
} |
|||
#endif
|
|||
} |
|||
|
|||
|
|||
void setup() |
|||
{ |
|||
SerialOpen(0, 115200); |
|||
LEDPIN_PINMODE; |
|||
POWERPIN_PINMODE; |
|||
BUZZERPIN_PINMODE; |
|||
STABLEPIN_PINMODE; |
|||
POWERPIN_OFF; |
|||
initOutput(); |
|||
readEEPROM(); |
|||
checkFirstTime(); |
|||
configureReceiver(); |
|||
initSensors(); |
|||
previousTime = micros(); |
|||
#if defined(GIMBAL)
|
|||
calibratingA = 400; |
|||
#endif
|
|||
calibratingG = 400; |
|||
#if defined(POWERMETER)
|
|||
for (uint8_t i = 0; i <= PMOTOR_SUM; i++) |
|||
pMeter[i] = 0; |
|||
#endif
|
|||
#if defined(GPS)
|
|||
SerialOpen(GPS_SERIAL, GPS_BAUD); |
|||
#endif
|
|||
#if defined(LCD_ETPP)
|
|||
initLCD(); |
|||
#elif defined(LCD_LCD03)
|
|||
initLCD(); |
|||
#endif
|
|||
#ifdef LCD_TELEMETRY_DEBUG
|
|||
telemetry_auto = 1; |
|||
#endif
|
|||
#ifdef LCD_CONF_DEBUG
|
|||
configurationLoop(); |
|||
#endif
|
|||
|
|||
|
|||
} |
|||
|
|||
// ******** Main Loop *********
|
|||
void loop() |
|||
{ |
|||
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
|||
uint8_t axis, i; |
|||
int16_t error, errorAngle; |
|||
int16_t delta, deltaSum; |
|||
int16_t PTerm, ITerm, DTerm; |
|||
static int16_t lastGyro[3] = { 0, 0, 0 }; |
|||
static int16_t delta1[3], delta2[3]; |
|||
static int16_t errorGyroI[3] = { 0, 0, 0 }; |
|||
static int16_t errorAngleI[2] = { 0, 0 }; |
|||
static uint32_t rcTime = 0; |
|||
static int16_t initialThrottleHold; |
|||
static int16_t errorAltitudeI = 0; |
|||
int16_t AltPID = 0; |
|||
static int16_t AltHold; |
|||
|
|||
#if defined(SPEKTRUM)
|
|||
if (rcFrameComplete) |
|||
computeRC(); |
|||
#endif
|
|||
|
|||
if (currentTime > rcTime) { // 50Hz
|
|||
rcTime = currentTime + 20000; |
|||
#if !(defined(SPEKTRUM) ||defined(BTSERIAL))
|
|||
computeRC(); |
|||
#endif
|
|||
// Failsafe routine - added by MIS
|
|||
#if defined(FAILSAFE)
|
|||
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { // Stabilize, and set Throttle to specified level
|
|||
for (i = 0; i < 3; i++) |
|||
rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
|
|||
rcData[THROTTLE] = FAILSAVE_THR0TTLE; |
|||
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
|
|||
armed = 0; //This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
|
|||
okToArm = 0; //to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
|
|||
} |
|||
failsafeEvents++; |
|||
} |
|||
failsafeCnt++; |
|||
#endif
|
|||
// end of failsave routine - next change is made with RcOptions setting
|
|||
if (rcData[THROTTLE] < MINCHECK) { |
|||
errorGyroI[ROLL] = 0; |
|||
errorGyroI[PITCH] = 0; |
|||
errorGyroI[YAW] = 0; |
|||
errorAngleI[ROLL] = 0; |
|||
errorAngleI[PITCH] = 0; |
|||
rcDelayCommand++; |
|||
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && armed == 0) { |
|||
if (rcDelayCommand == 20) |
|||
calibratingG = 400; |
|||
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) { |
|||
if (rcDelayCommand == 20) { |
|||
servo[0] = 1500; //we center the yaw gyro in conf mode
|
|||
writeServos(); |
|||
#if defined(LCD_CONF)
|
|||
configurationLoop(); //beginning LCD configuration
|
|||
#endif
|
|||
previousTime = micros(); |
|||
} |
|||
} |
|||
#if defined(InflightAccCalibration)
|
|||
else if (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK) { |
|||
if (rcDelayCommand == 20) { |
|||
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
|
|||
AccInflightCalibrationMeasurementDone = 0; |
|||
AccInflightCalibrationSavetoEEProm = 1; |
|||
} else { |
|||
AccInflightCalibrationArmed = !AccInflightCalibrationArmed; |
|||
if (AccInflightCalibrationArmed) { |
|||
blinkLED(10, 1, 2); |
|||
} else { |
|||
blinkLED(10, 10, 3); |
|||
} |
|||
} |
|||
} |
|||
} |
|||
#endif
|
|||
else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) { |
|||
if (((rcOptions1 & activate1[BOXARM]) |
|||
|| (rcOptions2 & activate2[BOXARM])) && okToArm) { |
|||
armed = 1; |
|||
headFreeModeHold = heading; |
|||
} else if (armed) |
|||
armed = 0; |
|||
rcDelayCommand = 0; |
|||
} else if ((rcData[YAW] < MINCHECK || rcData[ROLL] < MINCHECK) |
|||
&& armed == 1) { |
|||
if (rcDelayCommand == 20) |
|||
armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
|
|||
} else if ((rcData[YAW] > MAXCHECK || rcData[ROLL] > MAXCHECK) |
|||
&& rcData[PITCH] < MAXCHECK && armed == 0 && calibratingG == 0 && calibratedACC == 1) { |
|||
if (rcDelayCommand == 20) { |
|||
armed = 1; |
|||
headFreeModeHold = heading; |
|||
} |
|||
#ifdef LCD_TELEMETRY_AUTO
|
|||
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && armed == 0) { |
|||
if (rcDelayCommand == 20) { |
|||
if (telemetry_auto) { |
|||
telemetry_auto = 0; |
|||
telemetry = 0; |
|||
} else |
|||
telemetry_auto = 1; |
|||
} |
|||
#endif
|
|||
} else |
|||
rcDelayCommand = 0; |
|||
} else if (rcData[THROTTLE] > MAXCHECK && armed == 0) { |
|||
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=left, pitch=min
|
|||
if (rcDelayCommand == 20) |
|||
calibratingA = 400; |
|||
rcDelayCommand++; |
|||
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=right, pitch=min
|
|||
if (rcDelayCommand == 20) |
|||
calibratingM = 1; // MAG calibration request
|
|||
rcDelayCommand++; |
|||
} else if (rcData[PITCH] > MAXCHECK) { |
|||
accTrim[PITCH] += 2; |
|||
writeParams(); |
|||
#if defined(LED_RING)
|
|||
blinkLedRing(); |
|||
#endif
|
|||
} else if (rcData[PITCH] < MINCHECK) { |
|||
accTrim[PITCH] -= 2; |
|||
writeParams(); |
|||
#if defined(LED_RING)
|
|||
blinkLedRing(); |
|||
#endif
|
|||
} else if (rcData[ROLL] > MAXCHECK) { |
|||
accTrim[ROLL] += 2; |
|||
writeParams(); |
|||
#if defined(LED_RING)
|
|||
blinkLedRing(); |
|||
#endif
|
|||
} else if (rcData[ROLL] < MINCHECK) { |
|||
accTrim[ROLL] -= 2; |
|||
writeParams(); |
|||
#if defined(LED_RING)
|
|||
blinkLedRing(); |
|||
#endif
|
|||
} else { |
|||
rcDelayCommand = 0; |
|||
} |
|||
} |
|||
#ifdef LOG_VALUES
|
|||
if (cycleTime > cycleTimeMax) |
|||
cycleTimeMax = cycleTime; // remember highscore
|
|||
if (cycleTime < cycleTimeMin) |
|||
cycleTimeMin = cycleTime; // remember lowscore
|
|||
#endif
|
|||
|
|||
#if defined(InflightAccCalibration)
|
|||
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !((rcOptions1 & activate1[BOXARM]) || (rcOptions2 & activate2[BOXARM]))) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
|||
InflightcalibratingA = 50; |
|||
AccInflightCalibrationArmed = 0; |
|||
} |
|||
if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
|||
if (!AccInflightCalibrationArmed) { |
|||
AccInflightCalibrationArmed = 1; |
|||
InflightcalibratingA = 50; |
|||
} |
|||
} else if (AccInflightCalibrationMeasurementDone && armed == 0) { |
|||
AccInflightCalibrationArmed = 0; |
|||
AccInflightCalibrationMeasurementDone = 0; |
|||
AccInflightCalibrationSavetoEEProm = 1; |
|||
} |
|||
#endif
|
|||
|
|||
rcOptions1 = (rcData[AUX1] < 1300) + (1300 < rcData[AUX1] |
|||
&& rcData[AUX1] < 1700) * 2 + (rcData[AUX1] > 1700) * 4 + (rcData[AUX2] < 1300) * 8 + (1300 < rcData[AUX2] |
|||
&& rcData[AUX2] < 1700) * 16 + (rcData[AUX2] > 1700) * 32; |
|||
rcOptions2 = (rcData[AUX3] < 1300) + (1300 < rcData[AUX3] |
|||
&& rcData[AUX3] < 1700) * 2 + (rcData[AUX3] > 1700) * 4 + (rcData[AUX4] < 1300) * 8 + (1300 < rcData[AUX4] |
|||
&& rcData[AUX4] < 1700) * 16 + (rcData[AUX4] > 1700) * 32; |
|||
|
|||
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
|||
if (((rcOptions1 & activate1[BOXACC]) |
|||
|| (rcOptions2 & activate2[BOXACC]) |
|||
|| (failsafeCnt > 5 * FAILSAVE_DELAY)) && (ACC || nunchuk)) { |
|||
// bumpless transfer to Level mode
|
|||
if (!accMode) { |
|||
errorAngleI[ROLL] = 0; |
|||
errorAngleI[PITCH] = 0; |
|||
accMode = 1; |
|||
} |
|||
} else |
|||
accMode = 0; // modified by MIS for failsave support
|
|||
|
|||
if ((rcOptions1 & activate1[BOXARM]) == 0 || (rcOptions2 & activate2[BOXARM]) == 0) |
|||
okToArm = 1; |
|||
if (accMode == 1) |
|||
STABLEPIN_ON; |
|||
else |
|||
STABLEPIN_OFF; |
|||
|
|||
if (BARO) { |
|||
if ((rcOptions1 & activate1[BOXBARO]) |
|||
|| (rcOptions2 & activate2[BOXBARO])) { |
|||
if (baroMode == 0) { |
|||
baroMode = 1; |
|||
AltHold = EstAlt; |
|||
initialThrottleHold = rcCommand[THROTTLE]; |
|||
errorAltitudeI = 0; |
|||
} |
|||
} else |
|||
baroMode = 0; |
|||
} |
|||
if (MAG) { |
|||
if ((rcOptions1 & activate1[BOXMAG]) |
|||
|| (rcOptions2 & activate2[BOXMAG])) { |
|||
if (magMode == 0) { |
|||
magMode = 1; |
|||
magHold = heading; |
|||
} |
|||
} else |
|||
magMode = 0; |
|||
if ((rcOptions1 & activate1[BOXHEADFREE]) |
|||
|| (rcOptions2 & activate2[BOXHEADFREE])) { |
|||
if (headFreeMode == 0) { |
|||
headFreeMode = 1; |
|||
} |
|||
} else |
|||
headFreeMode = 0; |
|||
} |
|||
#if defined(GPS)
|
|||
if ((rcOptions1 & activate1[BOXGPSHOME]) |
|||
|| (rcOptions2 & activate2[BOXGPSHOME])) { |
|||
GPSModeHome = 1; |
|||
} else |
|||
GPSModeHome = 0; |
|||
if ((rcOptions1 & activate1[BOXGPSHOLD]) |
|||
|| (rcOptions2 & activate2[BOXGPSHOLD])) { |
|||
GPSModeHold = 1; |
|||
} else |
|||
GPSModeHold = 0; |
|||
#endif
|
|||
if ((rcOptions1 & activate1[BOXPASSTHRU]) |
|||
|| (rcOptions2 & activate2[BOXPASSTHRU])) { |
|||
passThruMode = 1; |
|||
} else |
|||
passThruMode = 0; |
|||
} |
|||
|
|||
computeIMU(); |
|||
// Measure loop rate just afer reading the sensors
|
|||
currentTime = micros(); |
|||
cycleTime = currentTime - previousTime; |
|||
previousTime = currentTime; |
|||
|
|||
if (MAG) { |
|||
if (abs(rcCommand[YAW]) < 70 && magMode) { |
|||
int16_t dif = heading - magHold; |
|||
if (dif <= -180) |
|||
dif += 360; |
|||
if (dif >= +180) |
|||
dif -= 360; |
|||
if (smallAngle25) |
|||
rcCommand[YAW] -= dif * P8[PIDMAG] / 30; //18 deg
|
|||
} else |
|||
magHold = heading; |
|||
} |
|||
|
|||
if (BARO) { |
|||
if (baroMode) { |
|||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) { |
|||
AltHold = EstAlt; |
|||
initialThrottleHold = rcCommand[THROTTLE]; |
|||
errorAltitudeI = 0; |
|||
} |
|||
//**** Alt. Set Point stabilization PID ****
|
|||
error = constrain(AltHold - EstAlt, -100, 100); // +/-10m, 1 decimeter accuracy
|
|||
errorAltitudeI += error; |
|||
errorAltitudeI = constrain(errorAltitudeI, -5000, 5000); |
|||
|
|||
PTerm = P8[PIDALT] * error / 10; // 16 bits is ok here
|
|||
|
|||
if (abs(error) > 5) // under 50cm error, we neutralize Iterm
|
|||
ITerm = (int32_t) I8[PIDALT] * errorAltitudeI / 4000; |
|||
else |
|||
ITerm = 0; |
|||
|
|||
AltPID = PTerm + ITerm; |
|||
|
|||
//AltPID is reduced, depending of the zVelocity magnitude
|
|||
AltPID = AltPID * (D8[PIDALT] - min(abs(zVelocity), D8[PIDALT] * 4 / 5)) / (D8[PIDALT] + 1); |
|||
debug3 = AltPID; |
|||
|
|||
rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID, -100, +100); |
|||
} |
|||
} |
|||
#if defined(GPS)
|
|||
if ((GPSModeHome == 1)) { |
|||
float radDiff = (GPS_directionToHome - heading) * 0.0174533f; |
|||
GPS_angle[ROLL] = constrain(P8[PIDGPS] * sin(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination
|
|||
GPS_angle[PITCH] = constrain(P8[PIDGPS] * cos(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // max inclination = D deg
|
|||
} else { |
|||
GPS_angle[ROLL] = 0; |
|||
GPS_angle[PITCH] = 0; |
|||
} |
|||
#endif
|
|||
|
|||
//**** PITCH & ROLL & YAW PID ****
|
|||
for (axis = 0; axis < 3; axis++) { |
|||
if (accMode == 1 && axis < 2) { //LEVEL MODE
|
|||
// 50 degrees max inclination
|
|||
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + accTrim[axis]; //16 bits is ok here
|
|||
#ifdef LEVEL_PDF
|
|||
PTerm = -(int32_t) angle[axis] * P8[PIDLEVEL] / 100; |
|||
#else
|
|||
PTerm = (int32_t) errorAngle *P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
|||
#endif
|
|||
PTerm = constrain(PTerm, -D8[PIDLEVEL], +D8[PIDLEVEL]); |
|||
|
|||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
|
|||
ITerm = ((int32_t) errorAngleI[axis] * I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
|||
} else { //ACRO MODE or YAW axis
|
|||
if (abs(rcCommand[axis]) < 350) |
|||
error = rcCommand[axis] * 10 * 8 / P8[axis]; //16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
|
|||
else |
|||
error = (int32_t) rcCommand[axis] * 10 * 8 / P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
|
|||
error -= gyroData[axis]; |
|||
|
|||
PTerm = rcCommand[axis]; |
|||
|
|||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
|
|||
if (abs(gyroData[axis]) > 640) |
|||
errorGyroI[axis] = 0; |
|||
ITerm = (errorGyroI[axis] / 125 * I8[axis]) >> 6; //16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
|||
} |
|||
if (abs(gyroData[axis]) < 160) |
|||
PTerm -= gyroData[axis] * dynP8[axis] / 10 / 8; //16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
|
|||
else |
|||
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; //32 bits is needed for calculation
|
|||
|
|||
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
|||
lastGyro[axis] = gyroData[axis]; |
|||
deltaSum = delta1[axis] + delta2[axis] + delta; |
|||
delta2[axis] = delta1[axis]; |
|||
delta1[axis] = delta; |
|||
|
|||
if (abs(deltaSum) < 640) |
|||
DTerm = (deltaSum * dynD8[axis]) >> 5; //16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
|
|||
else |
|||
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
|||
|
|||
axisPID[axis] = PTerm + ITerm - DTerm; |
|||
} |
|||
|
|||
mixTable(); |
|||
writeServos(); |
|||
writeMotors(); |
|||
|
|||
#if defined(GPS)
|
|||
while (SerialAvailable(GPS_SERIAL)) { |
|||
if (GPS_newFrame(SerialRead(GPS_SERIAL))) { |
|||
if (GPS_update == 1) |
|||
GPS_update = 0; |
|||
else |
|||
GPS_update = 1; |
|||
if (GPS_fix == 1 && GPS_numSat == 4) { |
|||
if (GPS_fix_home == 0) { |
|||
GPS_fix_home = 1; |
|||
GPS_latitude_home = GPS_latitude; |
|||
GPS_longitude_home = GPS_longitude; |
|||
} |
|||
GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome); |
|||
} |
|||
} |
|||
} |
|||
#endif
|
|||
} |
|||
@ -1,647 +0,0 @@ |
|||
|
|||
#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
|
|||
#define SERVO
|
|||
#endif
|
|||
|
|||
#if defined(GIMBAL)
|
|||
#define NUMBER_MOTOR 0
|
|||
#define PRI_SERVO_FROM 1 // use servo from 1 to 2
|
|||
#define PRI_SERVO_TO 2
|
|||
#elif defined(FLYING_WING)
|
|||
#define NUMBER_MOTOR 1
|
|||
#define PRI_SERVO_FROM 1 // use servo from 1 to 2
|
|||
#define PRI_SERVO_TO 2
|
|||
#elif defined(BI)
|
|||
#define NUMBER_MOTOR 2
|
|||
#define PRI_SERVO_FROM 5 // use servo from 5 to 6
|
|||
#define PRI_SERVO_TO 6
|
|||
#elif defined(TRI)
|
|||
#define NUMBER_MOTOR 3
|
|||
#define PRI_SERVO_FROM 5 // use only servo 5
|
|||
#define PRI_SERVO_TO 5
|
|||
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
|
|||
#define NUMBER_MOTOR 4
|
|||
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
|
|||
#define NUMBER_MOTOR 6
|
|||
#elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
|
|||
#define NUMBER_MOTOR 8
|
|||
#endif
|
|||
|
|||
#if defined(SERVO_TILT) && defined(CAMTRIG)
|
|||
#define SEC_SERVO_FROM 1 // use servo from 1 to 3
|
|||
#define SEC_SERVO_TO 3
|
|||
#else
|
|||
#if defined(SERVO_TILT)
|
|||
// if A0 and A1 is taken by motors, we can use A2 and 12 for Servo tilt
|
|||
#if defined(A0_A1_PIN_HEX) && (NUMBER_MOTOR == 6) && defined(PROMINI)
|
|||
#define SEC_SERVO_FROM 3 // use servo from 3 to 4
|
|||
#define SEC_SERVO_TO 4
|
|||
#else
|
|||
#define SEC_SERVO_FROM 1 // use servo from 1 to 2
|
|||
#define SEC_SERVO_TO 2
|
|||
#endif
|
|||
#endif
|
|||
#if defined(CAMTRIG)
|
|||
#define SEC_SERVO_FROM 3 // use servo 3
|
|||
#define SEC_SERVO_TO 3
|
|||
#endif
|
|||
#endif
|
|||
|
|||
|
|||
uint8_t PWM_PIN[8] = { MOTOR_ORDER }; |
|||
// so we need a servo pin array
|
|||
volatile uint8_t atomicServo[8] = { 125, 125, 125, 125, 125, 125, 125, 125 }; |
|||
|
|||
//for HEX Y6 and HEX6/HEX6X flat and for promini
|
|||
volatile uint8_t atomicPWM_PIN5_lowState; |
|||
volatile uint8_t atomicPWM_PIN5_highState; |
|||
volatile uint8_t atomicPWM_PIN6_lowState; |
|||
volatile uint8_t atomicPWM_PIN6_highState; |
|||
//for OCTO on promini
|
|||
volatile uint8_t atomicPWM_PINA2_lowState; |
|||
volatile uint8_t atomicPWM_PINA2_highState; |
|||
volatile uint8_t atomicPWM_PIN12_lowState; |
|||
volatile uint8_t atomicPWM_PIN12_highState; |
|||
|
|||
void writeServos() |
|||
{ |
|||
#if defined(SERVO)
|
|||
// write primary servos
|
|||
#if defined(PRI_SERVO_FROM)
|
|||
for (uint8_t i = (PRI_SERVO_FROM - 1); i < PRI_SERVO_TO; i++) { |
|||
atomicServo[i] = (servo[i] - 1000) >> 2; |
|||
} |
|||
#endif
|
|||
// write secundary servos
|
|||
#if defined(SEC_SERVO_FROM)
|
|||
for (uint8_t i = (SEC_SERVO_FROM - 1); i < SEC_SERVO_TO; i++) { |
|||
atomicServo[i] = (servo[i] - 1000) >> 2; |
|||
} |
|||
#endif
|
|||
#endif
|
|||
} |
|||
|
|||
void writeMotors() |
|||
{ // [1000;2000] => [125;250]
|
|||
#if defined(MEGA)
|
|||
#if (NUMBER_MOTOR > 0)
|
|||
OCR3C = motor[0] >> 3; // pin 3
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 1)
|
|||
OCR3A = motor[1] >> 3; // pin 5
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 2)
|
|||
OCR4A = motor[2] >> 3; // pin 6
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 3)
|
|||
OCR3B = motor[3] >> 3; // pin 2
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 4)
|
|||
OCR4B = motor[4] >> 3; // pin 7
|
|||
OCR4C = motor[5] >> 3; // pin 8
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 6)
|
|||
OCR2B = motor[6] >> 3; // pin 9
|
|||
OCR2A = motor[7] >> 3; // pin 10
|
|||
#endif
|
|||
#endif
|
|||
#if defined(PROMINI)
|
|||
#if (NUMBER_MOTOR > 0)
|
|||
#ifndef EXT_MOTOR_RANGE
|
|||
OCR1A = motor[0] >> 3; // pin 9
|
|||
#else
|
|||
OCR1A = ((motor[0] >> 2) - 250) + 2) |
|||
#endif
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 1)
|
|||
#ifndef EXT_MOTOR_RANGE
|
|||
OCR1B = motor[1] >> 3; // pin 10
|
|||
#else
|
|||
OCR1B = ((motor[1] >> 2) - 250) + 2) |
|||
#endif
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 2)
|
|||
#ifndef EXT_MOTOR_RANGE
|
|||
OCR2A = motor[2] >> 3; // pin 11
|
|||
#else
|
|||
OCR2A = ((motor[2] >> 2) - 250) + 2) |
|||
#endif
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 3)
|
|||
#ifndef EXT_MOTOR_RANGE
|
|||
OCR2B = motor[3] >> 3; // pin 3
|
|||
#else
|
|||
OCR2B = ((motor[3] >> 2) - 250) + 2) |
|||
#endif
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 4)
|
|||
atomicPWM_PIN5_highState = ((motor[5] - 1000) / 4.08) + 5; |
|||
atomicPWM_PIN5_lowState = 255 - atomicPWM_PIN5_highState; |
|||
atomicPWM_PIN6_highState = ((motor[4] - 1000) / 4.08) + 5; |
|||
atomicPWM_PIN6_lowState = 255 - atomicPWM_PIN6_highState; |
|||
#endif
|
|||
#if (NUMBER_MOTOR > 6)
|
|||
atomicPWM_PINA2_highState = ((motor[6] - 1000) / 4.08) + 5; |
|||
atomicPWM_PINA2_lowState = 255 - atomicPWM_PINA2_highState; |
|||
atomicPWM_PIN12_highState = ((motor[7] - 1000) / 4.08) + 5; |
|||
atomicPWM_PIN12_lowState = 255 - atomicPWM_PIN12_highState; |
|||
#endif
|
|||
#endif
|
|||
} |
|||
|
|||
void writeAllMotors(int16_t mc) { // Sends commands to all motors
|
|||
for (uint8_t i = 0; i < NUMBER_MOTOR; i++) |
|||
motor[i] = mc; |
|||
writeMotors(); |
|||
} |
|||
|
|||
void initOutput() { |
|||
#if defined(MEGA)
|
|||
for (uint8_t i = 0; i < NUMBER_MOTOR; i++) |
|||
pinMode(PWM_PIN[i], OUTPUT); |
|||
#if (NUMBER_MOTOR > 0)
|
|||
TCCR3A |= _BV(COM3C1); // connect pin 3 to timer 3 channel C
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 1)
|
|||
TCCR3A |= _BV(COM3A1); // connect pin 5 to timer 3 channel A
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 2)
|
|||
TCCR4A |= _BV(COM4A1); // connect pin 6 to timer 4 channel A
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 3)
|
|||
TCCR3A |= _BV(COM3B1); // connect pin 2 to timer 3 channel B
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 4)
|
|||
TCCR4A |= _BV(COM4B1); // connect pin 7 to timer 4 channel B
|
|||
TCCR4A |= _BV(COM4C1); // connect pin 8 to timer 4 channel C
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 6)
|
|||
TCCR2A |= _BV(COM2B1); // connect pin 9 to timer 2 channel B
|
|||
TCCR2A |= _BV(COM2A1); // connect pin 10 to timer 2 channel A
|
|||
#endif
|
|||
#endif
|
|||
#if defined(PROMINI)
|
|||
for (uint8_t i = 0; i < min(NUMBER_MOTOR, 4); i++) |
|||
pinMode(PWM_PIN[i], OUTPUT); |
|||
#if (NUMBER_MOTOR > 0)
|
|||
TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 1)
|
|||
TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 2)
|
|||
TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A
|
|||
#endif
|
|||
#if (NUMBER_MOTOR > 3)
|
|||
TCCR2A |= _BV(COM2B1); // connect pin 3 to timer 2 channel B
|
|||
#endif
|
|||
#endif
|
|||
|
|||
writeAllMotors(1000); |
|||
delay(300); |
|||
#if defined(SERVO)
|
|||
initializeServo(); |
|||
#endif
|
|||
#if (NUMBER_MOTOR == 6) && defined(PROMINI)
|
|||
initializeSoftPWM(); |
|||
#if defined(A0_A1_PIN_HEX)
|
|||
pinMode(5, INPUT); |
|||
pinMode(6, INPUT); // we reactivate the INPUT affectation for these two PINs
|
|||
pinMode(A0, OUTPUT); |
|||
pinMode(A1, OUTPUT); |
|||
#endif
|
|||
#elif (NUMBER_MOTOR == 8) && defined(PROMINI)
|
|||
initializeSoftPWM(); |
|||
#if defined(A0_A1_PIN_HEX)
|
|||
pinMode(5, INPUT); |
|||
pinMode(6, INPUT); // we reactivate the INPUT affectation for these two PINs
|
|||
pinMode(A0, OUTPUT); |
|||
pinMode(A1, OUTPUT); |
|||
#endif
|
|||
#endif
|
|||
} |
|||
|
|||
#if defined(SERVO)
|
|||
void initializeServo() { |
|||
#if (PRI_SERVO_FROM == 1) || (SEC_SERVO_FROM == 1)
|
|||
SERVO_1_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2)
|
|||
SERVO_2_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3)
|
|||
SERVO_3_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4)
|
|||
SERVO_4_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5)
|
|||
SERVO_5_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6)
|
|||
SERVO_6_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7)
|
|||
SERVO_7_PINMODE; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8)
|
|||
SERVO_8_PINMODE; |
|||
#endif
|
|||
|
|||
TCCR0A = 0; // normal counting mode
|
|||
TIMSK0 |= (1 << OCIE0A); // Enable CTC interrupt
|
|||
|
|||
// timer 0B for hex with servo tilt
|
|||
#if (NUMBER_MOTOR == 6) && defined(PROMINI)
|
|||
TIMSK0 |= (1 << OCIE0B); |
|||
#endif
|
|||
} |
|||
|
|||
// ****servo yaw with a 50Hz refresh rate****
|
|||
// prescaler is set by default to 64 on Timer0
|
|||
// Duemilanove : 16MHz / 64 => 4 us
|
|||
// 256 steps = 1 counter cycle = 1024 us
|
|||
ISR(TIMER0_COMPA_vect) { |
|||
static uint8_t state = 0; |
|||
static uint8_t count; |
|||
if (state == 0) { |
|||
//http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/
|
|||
|
|||
#if (PRI_SERVO_FROM == 1) || (SEC_SERVO_FROM == 1)
|
|||
SERVO_1_PIN_HIGH; |
|||
#endif
|
|||
OCR0A += 250; // 1000 us
|
|||
state++; |
|||
} else if (state == 1) { |
|||
OCR0A += atomicServo[0]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 2) { |
|||
#if (PRI_SERVO_FROM == 1) || (SEC_SERVO_FROM == 1)
|
|||
SERVO_1_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2)
|
|||
SERVO_2_PIN_HIGH; |
|||
#endif
|
|||
OCR0A += 250; // 1000 us
|
|||
state++; |
|||
} else if (state == 3) { |
|||
OCR0A += atomicServo[1]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 4) { |
|||
#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2)
|
|||
SERVO_2_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3)
|
|||
SERVO_3_PIN_HIGH; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 5) { |
|||
OCR0A += atomicServo[2]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 6) { |
|||
#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3)
|
|||
SERVO_3_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4)
|
|||
SERVO_4_PIN_HIGH; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 7) { |
|||
OCR0A += atomicServo[3]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 8) { |
|||
#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4)
|
|||
SERVO_4_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5)
|
|||
SERVO_5_PIN_HIGH;; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 9) { |
|||
OCR0A += atomicServo[4]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 10) { |
|||
#if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5)
|
|||
SERVO_5_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6)
|
|||
SERVO_6_PIN_HIGH;; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 11) { |
|||
OCR0A += atomicServo[5]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 12) { |
|||
#if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6)
|
|||
SERVO_6_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7)
|
|||
SERVO_7_PIN_HIGH; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 13) { |
|||
OCR0A += atomicServo[6]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 14) { |
|||
#if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7)
|
|||
SERVO_7_PIN_LOW; |
|||
#endif
|
|||
#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8)
|
|||
SERVO_8_PIN_HIGH; |
|||
#endif
|
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 15) { |
|||
OCR0A += atomicServo[7]; // 1000 + [0-1020] us
|
|||
state++; |
|||
} else if (state == 16) { |
|||
#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8)
|
|||
SERVO_8_PIN_LOW; |
|||
#endif
|
|||
count = 2; |
|||
state++; |
|||
OCR0A += 250; // 1000 us
|
|||
} else if (state == 17) { |
|||
if (count > 0) |
|||
count--; |
|||
else |
|||
state = 0; |
|||
OCR0A += 250; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
#if (NUMBER_MOTOR > 4) && defined(PROMINI)
|
|||
void initializeSoftPWM() { |
|||
// if there are servos its alredy done
|
|||
#if (NUMBER_MOTOR == 6) && not defined(SERVO)
|
|||
TCCR0A = 0; // normal counting mode
|
|||
TIMSK0 |= (1 << OCIE0B); // Enable CTC interrupt
|
|||
#endif
|
|||
|
|||
#if (NUMBER_MOTOR > 6)
|
|||
TCCR0A = 0; // normal counting mode
|
|||
TIMSK0 |= (1 << OCIE0A); // Enable CTC interrupt
|
|||
TIMSK0 |= (1 << OCIE0B); |
|||
#endif
|
|||
} |
|||
|
|||
// HEXA with just OCR0B
|
|||
ISR(TIMER0_COMPB_vect) { |
|||
static uint8_t state = 0; |
|||
if (state == 0) { |
|||
#if not defined(A0_A1_PIN_HEX)
|
|||
PORTD |= 1 << 5; //digital PIN 5 high
|
|||
#else
|
|||
PORTC |= 1 << 0; //PIN A0
|
|||
#endif
|
|||
OCR0B += atomicPWM_PIN5_highState; |
|||
state = 1; |
|||
} else if (state == 1) { |
|||
#if not defined(A0_A1_PIN_HEX)
|
|||
PORTD &= ~(1 << 6); |
|||
#else
|
|||
PORTC &= ~(1 << 1); |
|||
#endif
|
|||
OCR0B += atomicPWM_PIN6_lowState; |
|||
state = 2; |
|||
} else if (state == 2) { |
|||
#if not defined(A0_A1_PIN_HEX)
|
|||
PORTD |= 1 << 6; |
|||
#else
|
|||
PORTC |= 1 << 1; //PIN A1
|
|||
#endif
|
|||
OCR0B += atomicPWM_PIN6_highState; |
|||
state = 3; |
|||
} else if (state == 3) { |
|||
#if not defined(A0_A1_PIN_HEX)
|
|||
PORTD &= ~(1 << 5); //digital PIN 5 low
|
|||
#else
|
|||
PORTC &= ~(1 << 0); |
|||
#endif
|
|||
OCR0B += atomicPWM_PIN5_lowState; |
|||
state = 0; |
|||
} |
|||
} |
|||
|
|||
//the same with digital PIN A2 & 12 OCR0A counter for OCTO
|
|||
#if (NUMBER_MOTOR > 6)
|
|||
ISR(TIMER0_COMPA_vect) { |
|||
static uint8_t state = 0; |
|||
if (state == 0) { |
|||
PORTC |= 1 << 2; //digital PIN A2 high
|
|||
OCR0A += atomicPWM_PINA2_highState; |
|||
state = 1; |
|||
} else if (state == 1) { |
|||
PORTB &= ~(1 << 4); //digital PIN 12 low
|
|||
OCR0A += atomicPWM_PIN12_lowState; |
|||
state = 2; |
|||
} else if (state == 2) { |
|||
PORTB |= 1 << 4; //digital PIN 12 high
|
|||
OCR0A += atomicPWM_PIN12_highState; |
|||
state = 3; |
|||
} else if (state == 3) { |
|||
PORTC &= ~(1 << 2); //digital PIN A2 low
|
|||
OCR0A += atomicPWM_PINA2_lowState; |
|||
state = 0; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
#endif
|
|||
|
|||
void mixTable() { |
|||
int16_t maxMotor; |
|||
uint8_t i, axis; |
|||
static uint8_t camCycle = 0; |
|||
static uint8_t camState = 0; |
|||
static uint32_t camTime = 0; |
|||
|
|||
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z
|
|||
|
|||
#if NUMBER_MOTOR > 3
|
|||
//prevent "yaw jump" during yaw correction
|
|||
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); |
|||
#endif
|
|||
#ifdef BI
|
|||
motor[0] = PIDMIX(+1, 0, 0); //LEFT
|
|||
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
|
|||
servo[4] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
|
|||
servo[5] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
|
|||
#endif
|
|||
#ifdef TRI
|
|||
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
|
|||
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
|
|||
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
|
|||
servo[4] = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
|||
#endif
|
|||
#ifdef QUADP
|
|||
motor[0] = PIDMIX(0, +1, -1); //REAR
|
|||
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
|
|||
motor[2] = PIDMIX(+1, 0, +1); //LEFT
|
|||
motor[3] = PIDMIX(0, -1, -1); //FRONT
|
|||
#endif
|
|||
#ifdef QUADX
|
|||
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
|||
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
|||
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
|||
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
|||
#endif
|
|||
#ifdef Y4
|
|||
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
|
|||
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
|
|||
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
|
|||
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
|
|||
#endif
|
|||
#ifdef Y6
|
|||
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
|
|||
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
|
|||
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
|
|||
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
|
|||
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
|
|||
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
|
|||
#endif
|
|||
#ifdef HEX6
|
|||
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
|||
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
|
|||
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
|
|||
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
|||
motor[4] = PIDMIX(+0, -1, +1); //FRONT
|
|||
motor[5] = PIDMIX(+0, +1, -1); //REAR
|
|||
#endif
|
|||
#ifdef HEX6X
|
|||
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
|||
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
|
|||
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
|
|||
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
|||
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
|
|||
motor[5] = PIDMIX(+1, +0, +1); //LEFT
|
|||
#endif
|
|||
#ifdef OCTOX8
|
|||
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
|||
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
|||
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
|||
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
|||
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
|
|||
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
|
|||
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
|
|||
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
|
|||
#endif
|
|||
#ifdef OCTOFLATP
|
|||
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
|
|||
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
|
|||
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
|
|||
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
|
|||
motor[4] = PIDMIX(+0, -1, -1); //FRONT
|
|||
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
|
|||
motor[6] = PIDMIX(+0, +1, -1); //REAR
|
|||
motor[7] = PIDMIX(+1, +0, -1); //LEFT
|
|||
#endif
|
|||
#ifdef OCTOFLATX
|
|||
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
|
|||
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
|
|||
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
|
|||
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
|
|||
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
|
|||
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
|
|||
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
|
|||
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
|
|||
#endif
|
|||
|
|||
#ifdef SERVO_TILT
|
|||
if ((rcOptions1 & activate1[BOXCAMSTAB]) |
|||
|| (rcOptions2 & activate2[BOXCAMSTAB])) { |
|||
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); |
|||
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); |
|||
} else { |
|||
// to use it with A0_A1_PIN_HEX
|
|||
#if defined(A0_A1_PIN_HEX) && (NUMBER_MOTOR == 6) && defined(PROMINI)
|
|||
servo[2] = constrain(TILT_PITCH_MIDDLE + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); |
|||
servo[3] = constrain(TILT_ROLL_MIDDLE + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); |
|||
#else
|
|||
servo[0] = constrain(TILT_PITCH_MIDDLE + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); |
|||
servo[1] = constrain(TILT_ROLL_MIDDLE + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); |
|||
#endif
|
|||
} |
|||
#endif
|
|||
#ifdef GIMBAL
|
|||
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX); |
|||
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX); |
|||
#endif
|
|||
#ifdef FLYING_WING
|
|||
motor[0] = rcCommand[THROTTLE]; |
|||
if (passThruMode) { // use raw stick values to drive output
|
|||
servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC), WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
|||
servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC), WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
|||
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
|
|||
servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL], WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
|||
servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
|||
} |
|||
#endif
|
|||
#if defined(CAMTRIG)
|
|||
if (camCycle == 1) { |
|||
if (camState == 0) { |
|||
servo[2] = CAM_SERVO_HIGH; |
|||
camState = 1; |
|||
camTime = millis(); |
|||
} else if (camState == 1) { |
|||
if ((millis() - camTime) > CAM_TIME_HIGH) { |
|||
servo[2] = CAM_SERVO_LOW; |
|||
camState = 2; |
|||
camTime = millis(); |
|||
} |
|||
} else { //camState ==2
|
|||
if ((millis() - camTime) > CAM_TIME_LOW) { |
|||
camState = 0; |
|||
camCycle = 0; |
|||
} |
|||
} |
|||
} |
|||
if ((rcOptions1 & activate1[BOXCAMTRIG]) |
|||
|| (rcOptions1 & activate2[BOXCAMTRIG])) |
|||
camCycle = 1; |
|||
#endif
|
|||
|
|||
maxMotor = motor[0]; |
|||
for (i = 1; i < NUMBER_MOTOR; i++) |
|||
if (motor[i] > maxMotor) |
|||
maxMotor = motor[i]; |
|||
for (i = 0; i < NUMBER_MOTOR; i++) { |
|||
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
|
|||
motor[i] -= maxMotor - MAXTHROTTLE; |
|||
motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE); |
|||
if ((rcData[THROTTLE]) < MINCHECK) |
|||
#ifndef MOTOR_STOP
|
|||
motor[i] = MINTHROTTLE; |
|||
#else
|
|||
motor[i] = MINCOMMAND; |
|||
#endif
|
|||
if (armed == 0) |
|||
motor[i] = MINCOMMAND; |
|||
} |
|||
|
|||
#if (LOG_VALUES == 2) || defined(POWERMETER_SOFT)
|
|||
uint32_t amp; |
|||
/* true cubic function; when divided by vbat_max=126 (12.6V) for 3 cell battery this gives maximum value of ~ 500 */ |
|||
/* Lookup table moved to PROGMEM 11/21/2001 by Danal */ |
|||
static uint16_t amperes[64] = { |
|||
0, 2, 6, 15, 30, 52, 82, 123, 175, 240, 320, 415, 528, 659, 811, 984, 1181, 1402, 1648, 1923, 2226, 2559, 2924, 3322, 3755, 4224, 4730, 5276, 5861, 6489, 7160, 7875, 8637, 9446, 10304, 11213, 12173, 13187, 14256, 15381, 16564, 17805, 19108, 20472, 21900, 23392, 24951, 26578, 28274, 30041, 31879, 33792, 35779, 37843, 39984, 42205, 44507, 46890, 49358, 51910, 54549, 57276, 60093, 63000}; |
|||
|
|||
if (vbat) { // by all means - must avoid division by zero
|
|||
for (uint8_t i = 0; i < NUMBER_MOTOR; i++) { |
|||
amp = amperes[((motor[i] - 1000) >> 4)] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
|
|||
#if (LOG_VALUES == 2)
|
|||
pMeter[i] += amp; // sum up over time the mapped ESC input
|
|||
#endif
|
|||
#if defined(POWERMETER_SOFT)
|
|||
pMeter[PMOTOR_SUM] += amp; // total sum over all motors
|
|||
#endif
|
|||
} |
|||
} |
|||
#endif
|
|||
} |
|||
@ -1,285 +0,0 @@ |
|||
volatile uint16_t rcValue[18] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; // interval [1000;2000]
|
|||
#if defined(SERIAL_SUM_PPM)
|
|||
static uint8_t rcChannel[8] = {SERIAL_SUM_PPM}; |
|||
#elif defined(SBUS)
|
|||
// for 16 + 2 Channels SBUS. The 10 extra channels 8->17 are not used by MultiWii, but it should be easy to integrate them.
|
|||
static uint8_t rcChannel[18] = {PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11,12,13,14,15,16,17}; |
|||
static uint16_t sbusIndex=0; |
|||
#else
|
|||
static uint8_t rcChannel[8] = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN}; |
|||
#endif
|
|||
#if defined(SPEKTRUM)
|
|||
#define SPEK_MAX_CHANNEL 7
|
|||
#define SPEK_FRAME_SIZE 16
|
|||
#if (SPEKTRUM == 1024)
|
|||
#define SPEK_CHAN_SHIFT 2 // Assumes 10 bit frames, that is 1024 mode.
|
|||
#define SPEK_CHAN_MASK 0x03 // Assumes 10 bit frames, that is 1024 mode.
|
|||
#endif
|
|||
#if (SPEKTRUM == 2048)
|
|||
#define SPEK_CHAN_SHIFT 3 // Assumes 11 bit frames, that is 2048 mode.
|
|||
#define SPEK_CHAN_MASK 0x07 // Assumes 11 bit frames, that is 2048 mode.
|
|||
#endif
|
|||
volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; |
|||
#endif
|
|||
|
|||
// Configure each rc pin for PCINT
|
|||
void configureReceiver() { |
|||
#if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM) && !defined(SBUS) && !defined(BTSERIAL)
|
|||
#if defined(PROMINI)
|
|||
// PCINT activated only for specific pin inside [D0-D7] , [D2 D4 D5 D6 D7] for this multicopter
|
|||
PORTD = (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTD (no high impedence PINs)
|
|||
PCMSK2 |= (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7); |
|||
PCICR = (1<<2) ; // PCINT activated only for the port dealing with [D0-D7] PINs on port B
|
|||
#if defined(RCAUXPIN)
|
|||
PCICR |= (1<<0) ; // PCINT activated also for PINS [D8-D13] on port B
|
|||
#if defined(RCAUXPIN8)
|
|||
PCMSK0 = (1<<0); |
|||
#endif
|
|||
#if defined(RCAUXPIN12)
|
|||
PCMSK0 = (1<<4); |
|||
#endif
|
|||
#endif
|
|||
#endif
|
|||
#if defined(MEGA)
|
|||
// PCINT activated only for specific pin inside [A8-A15]
|
|||
DDRK = 0; // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)
|
|||
PORTK = (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTK
|
|||
PCMSK2 |= (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7); |
|||
PCICR = 1<<2; // PCINT activated only for PORTK dealing with [A8-A15] PINs
|
|||
#endif
|
|||
#endif
|
|||
#if defined(SERIAL_SUM_PPM)
|
|||
PPM_PIN_INTERRUPT; |
|||
#endif
|
|||
#if defined (SPEKTRUM)
|
|||
SerialOpen(1,115200); |
|||
#endif
|
|||
#if defined(SBUS)
|
|||
SerialOpen(1,100000); |
|||
#endif
|
|||
} |
|||
|
|||
#if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM) && !defined(SBUS) && !defined(BTSERIAL)
|
|||
ISR(PCINT2_vect) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a digital pin [D2-D7]
|
|||
uint8_t mask; |
|||
uint8_t pin; |
|||
uint16_t cTime,dTime; |
|||
static uint16_t edgeTime[8]; |
|||
static uint8_t PCintLast; |
|||
|
|||
#if defined(PROMINI)
|
|||
pin = PIND; // PIND indicates the state of each PIN for the arduino port dealing with [D0-D7] digital pins (8 bits variable)
|
|||
#endif
|
|||
#if defined(MEGA)
|
|||
pin = PINK; // PINK indicates the state of each PIN for the arduino port dealing with [A8-A15] digital pins (8 bits variable)
|
|||
#endif
|
|||
mask = pin ^ PCintLast; // doing a ^ between the current interruption and the last one indicates wich pin changed
|
|||
sei(); // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely
|
|||
PCintLast = pin; // we memorize the current state of all PINs [D0-D7]
|
|||
|
|||
cTime = micros(); // micros() return a uint32_t, but it is not usefull to keep the whole bits => we keep only 16 bits
|
|||
|
|||
// mask is pins [D0-D7] that have changed // the principle is the same on the MEGA for PORTK and [A8-A15] PINs
|
|||
// chan = pin sequence of the port. chan begins at D2 and ends at D7
|
|||
if (mask & 1<<2) //indicates the bit 2 of the arduino port [D0-D7], that is to say digital pin 2, if 1 => this pin has just changed
|
|||
if (!(pin & 1<<2)) { //indicates if the bit 2 of the arduino port [D0-D7] is not at a high state (so that we match here only descending PPM pulse)
|
|||
dTime = cTime-edgeTime[2]; if (900<dTime && dTime<2200) rcValue[2] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin
|
|||
} else edgeTime[2] = cTime; // if the bit 2 of the arduino port [D0-D7] is at a high state (ascending PPM pulse), we memorize the time
|
|||
if (mask & 1<<4) //same principle for other channels // avoiding a for() is more than twice faster, and it's important to minimize execution time in ISR
|
|||
if (!(pin & 1<<4)) { |
|||
dTime = cTime-edgeTime[4]; if (900<dTime && dTime<2200) rcValue[4] = dTime; |
|||
} else edgeTime[4] = cTime; |
|||
if (mask & 1<<5) |
|||
if (!(pin & 1<<5)) { |
|||
dTime = cTime-edgeTime[5]; if (900<dTime && dTime<2200) rcValue[5] = dTime; |
|||
} else edgeTime[5] = cTime; |
|||
if (mask & 1<<6) |
|||
if (!(pin & 1<<6)) { |
|||
dTime = cTime-edgeTime[6]; if (900<dTime && dTime<2200) rcValue[6] = dTime; |
|||
} else edgeTime[6] = cTime; |
|||
if (mask & 1<<7) |
|||
if (!(pin & 1<<7)) { |
|||
dTime = cTime-edgeTime[7]; if (900<dTime && dTime<2200) rcValue[7] = dTime; |
|||
} else edgeTime[7] = cTime; |
|||
#if defined(MEGA)
|
|||
if (mask & 1<<0) |
|||
if (!(pin & 1<<0)) { |
|||
dTime = cTime-edgeTime[0]; if (900<dTime && dTime<2200) rcValue[0] = dTime; |
|||
} else edgeTime[0] = cTime; |
|||
if (mask & 1<<1) |
|||
if (!(pin & 1<<1)) { |
|||
dTime = cTime-edgeTime[1]; if (900<dTime && dTime<2200) rcValue[1] = dTime; |
|||
} else edgeTime[1] = cTime; |
|||
if (mask & 1<<3) |
|||
if (!(pin & 1<<3)) { |
|||
dTime = cTime-edgeTime[3]; if (900<dTime && dTime<2200) rcValue[3] = dTime; |
|||
} else edgeTime[3] = cTime; |
|||
#endif
|
|||
#if defined(FAILSAFE)
|
|||
if (mask & 1<<THROTTLEPIN) { // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter - added by MIS
|
|||
if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; } |
|||
#endif
|
|||
} |
|||
|
|||
#if defined(RCAUXPIN)
|
|||
/* this ISR is a simplication of the previous one for PROMINI on port D
|
|||
it's simplier because we know the interruption deals only with on PIN: |
|||
bit 0 of PORT B, ie Arduino PIN 8 |
|||
or bit 4 of PORTB, ie Arduino PIN 12 |
|||
=> no need to check which PIN has changed */ |
|||
ISR(PCINT0_vect) { |
|||
uint8_t pin; |
|||
uint16_t cTime,dTime; |
|||
static uint16_t edgeTime; |
|||
|
|||
pin = PINB; |
|||
sei(); |
|||
cTime = micros(); |
|||
#if defined(RCAUXPIN8)
|
|||
if (!(pin & 1<<0)) { //indicates if the bit 0 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse)
|
|||
#endif
|
|||
#if defined(RCAUXPIN12)
|
|||
if (!(pin & 1<<4)) { //indicates if the bit 4 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse)
|
|||
#endif
|
|||
dTime = cTime-edgeTime; if (900<dTime && dTime<2200) rcValue[0] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin
|
|||
} else edgeTime = cTime; // if the bit 2 is at a high state (ascending PPM pulse), we memorize the time
|
|||
} |
|||
#endif
|
|||
#endif
|
|||
|
|||
#if defined(SERIAL_SUM_PPM)
|
|||
void rxInt() { |
|||
uint16_t now,diff; |
|||
static uint16_t last = 0; |
|||
static uint8_t chan = 0; |
|||
|
|||
now = micros(); |
|||
diff = now - last; |
|||
last = now; |
|||
if(diff>3000) chan = 0; |
|||
else { |
|||
if(900<diff && diff<2200 && chan<8 ) { //Only if the signal is between these values it is valid, otherwise the failsafe counter should move up
|
|||
rcValue[chan] = diff; |
|||
#if defined(FAILSAFE)
|
|||
if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // clear FailSafe counter - added by MIS //incompatible to quadroppm
|
|||
#endif
|
|||
} |
|||
chan++; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
#if defined(SPEKTRUM)
|
|||
ISR(SPEK_SERIAL_VECT) { |
|||
uint32_t spekTime; |
|||
static uint32_t spekTimeLast, spekTimeInterval; |
|||
static uint8_t spekFramePosition; |
|||
spekTime=micros(); |
|||
spekTimeInterval = spekTime - spekTimeLast; |
|||
spekTimeLast = spekTime; |
|||
if (spekTimeInterval > 5000) spekFramePosition = 0; |
|||
spekFrame[spekFramePosition] = SPEK_DATA_REG; |
|||
if (spekFramePosition == SPEK_FRAME_SIZE - 1) { |
|||
rcFrameComplete = 1; |
|||
#if defined(FAILSAFE)
|
|||
if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // clear FailSafe counter
|
|||
#endif
|
|||
} else { |
|||
spekFramePosition++; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
#if defined(SBUS)
|
|||
void readSBus(){ |
|||
#define SBUS_SYNCBYTE 0x0F // Not 100% sure: at the beginning of coding it was 0xF0 !!!
|
|||
static uint16_t sbus[25]={0}; |
|||
while(SerialAvailable(1)){ |
|||
int val = SerialRead(1); |
|||
if(sbusIndex==0 && val != SBUS_SYNCBYTE) |
|||
continue; |
|||
sbus[sbusIndex++] = val; |
|||
if(sbusIndex==25){ |
|||
sbusIndex=0; |
|||
rcValue[0] = ((sbus[1]|sbus[2]<< 8) & 0x07FF)/2+976; // Perhaps you may change the term "/2+976" -> center will be 1486
|
|||
rcValue[1] = ((sbus[2]>>3|sbus[3]<<5) & 0x07FF)/2+976; |
|||
rcValue[2] = ((sbus[3]>>6|sbus[4]<<2|sbus[5]<<10) & 0x07FF)/2+976; |
|||
rcValue[3] = ((sbus[5]>>1|sbus[6]<<7) & 0x07FF)/2+976; |
|||
rcValue[4] = ((sbus[6]>>4|sbus[7]<<4) & 0x07FF)/2+976; |
|||
rcValue[5] = ((sbus[7]>>7|sbus[8]<<1|sbus[9]<<9) & 0x07FF)/2+976; |
|||
rcValue[6] = ((sbus[9]>>2|sbus[10]<<6) & 0x07FF)/2+976; |
|||
rcValue[7] = ((sbus[10]>>5|sbus[11]<<3) & 0x07FF)/2+976; // & the other 8 + 2 channels if you need them
|
|||
//The following lines: If you need more than 8 channels, max 16 analog + 2 digital. Must comment the not needed channels!
|
|||
rcValue[8] = ((sbus[12]|sbus[13]<< 8) & 0x07FF)/2+976; |
|||
rcValue[9] = ((sbus[13]>>3|sbus[14]<<5) & 0x07FF)/2+976; |
|||
rcValue[10] = ((sbus[14]>>6|sbus[15]<<2|sbus[16]<<10) & 0x07FF)/2+976; |
|||
rcValue[11] = ((sbus[16]>>1|sbus[17]<<7) & 0x07FF)/2+976; |
|||
rcValue[12] = ((sbus[17]>>4|sbus[18]<<4) & 0x07FF)/2+976; |
|||
rcValue[13] = ((sbus[18]>>7|sbus[19]<<1|sbus[20]<<9) & 0x07FF)/2+976; |
|||
rcValue[14] = ((sbus[20]>>2|sbus[21]<<6) & 0x07FF)/2+976; |
|||
rcValue[15] = ((sbus[21]>>5|sbus[22]<<3) & 0x07FF)/2+976; |
|||
// now the two Digital-Channels
|
|||
if ((sbus[23]) & 0x0001) rcValue[16] = 2000; else rcValue[16] = 1000; |
|||
if ((sbus[23] >> 1) & 0x0001) rcValue[17] = 2000; else rcValue[17] = 1000; |
|||
|
|||
// Failsafe: there is one Bit in the SBUS-protocol (Byte 25, Bit 4) whitch is the failsafe-indicator-bit
|
|||
#if defined(FAILSAFE)
|
|||
if (!((sbus[23] >> 3) & 0x0001)) |
|||
{if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;} // clear FailSafe counter
|
|||
#endif
|
|||
} |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
uint16_t readRawRC(uint8_t chan) { |
|||
uint16_t data; |
|||
uint8_t oldSREG; |
|||
oldSREG = SREG; cli(); // Let's disable interrupts
|
|||
data = rcValue[rcChannel[chan]]; // Let's copy the data Atomically
|
|||
#if defined(SPEKTRUM)
|
|||
static uint32_t spekChannelData[SPEK_MAX_CHANNEL]; |
|||
if (rcFrameComplete) { |
|||
for (uint8_t b = 3; b < SPEK_FRAME_SIZE; b += 2) { |
|||
uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> SPEK_CHAN_SHIFT); |
|||
if (spekChannel < SPEK_MAX_CHANNEL) spekChannelData[spekChannel] = (long(spekFrame[b - 1] & SPEK_CHAN_MASK) << 8) + spekFrame[b]; |
|||
} |
|||
rcFrameComplete = 0; |
|||
} |
|||
#endif
|
|||
SREG = oldSREG; sei();// Let's enable the interrupts
|
|||
#if defined(SPEKTRUM)
|
|||
static uint8_t spekRcChannelMap[SPEK_MAX_CHANNEL] = {1,2,3,0,4,5,6}; |
|||
if (chan >= SPEK_MAX_CHANNEL) { |
|||
data = 1500; |
|||
} else { |
|||
#if (SPEKTRUM == 1024)
|
|||
data = 988 + spekChannelData[spekRcChannelMap[chan]]; // 1024 mode
|
|||
#endif
|
|||
#if (SPEKTRUM == 2048)
|
|||
data = 988 + (spekChannelData[spekRcChannelMap[chan]] >> 1); // 2048 mode
|
|||
#endif
|
|||
} |
|||
#endif
|
|||
return data; // We return the value correctly copied when the IRQ's where disabled
|
|||
} |
|||
|
|||
void computeRC() { |
|||
static int16_t rcData4Values[8][4], rcDataMean[8]; |
|||
static uint8_t rc4ValuesIndex = 0; |
|||
uint8_t chan,a,ind; |
|||
|
|||
#if defined(SBUS)
|
|||
readSBus(); |
|||
#endif
|
|||
rc4ValuesIndex++; |
|||
for (chan = 0; chan < 8; chan++) { |
|||
rcData4Values[chan][rc4ValuesIndex%4] = readRawRC(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; |
|||
} |
|||
} |
|||
|
|||
1207
mw-svn/Sensors.cpp
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
@ -1,486 +0,0 @@ |
|||
// *******************************************************
|
|||
// Interrupt driven UART transmitter - using a ring buffer
|
|||
// *******************************************************
|
|||
static uint8_t head, tail; |
|||
static uint8_t buf[256]; // 256 is choosen to avoid modulo operations on 8 bits pointers
|
|||
void serialize16(int16_t a) |
|||
{ |
|||
buf[head++] = a; |
|||
buf[head++] = a >> 8 & 0xff; |
|||
} |
|||
|
|||
void serialize8(uint8_t a) |
|||
{ |
|||
buf[head++] = a; |
|||
} |
|||
|
|||
ISR_UART { |
|||
UDR0 = buf[tail++]; // Transmit next byte in the ring
|
|||
if (tail == head) // Check if all data is transmitted
|
|||
UCSR0B &= ~(1 << UDRIE0); // Disable transmitter UDRE interrupt
|
|||
} |
|||
|
|||
void UartSendData() |
|||
{ // Data transmission acivated when the ring is not empty
|
|||
UCSR0B |= (1 << UDRIE0); // Enable transmitter UDRE interrupt
|
|||
} |
|||
|
|||
void serialCom() |
|||
{ |
|||
int16_t a; |
|||
uint8_t i; |
|||
|
|||
if (SerialAvailable(0)) { |
|||
switch (SerialRead(0)) { |
|||
#ifdef BTSERIAL
|
|||
case 'K': //receive RC data from Bluetooth Serial adapter as a remote
|
|||
rcData[THROTTLE] = (SerialRead(0) * 4) + 1000; |
|||
rcData[ROLL] = (SerialRead(0) * 4) + 1000; |
|||
rcData[PITCH] = (SerialRead(0) * 4) + 1000; |
|||
rcData[YAW] = (SerialRead(0) * 4) + 1000; |
|||
rcData[AUX1] = (SerialRead(0) * 4) + 1000; |
|||
break; |
|||
#endif
|
|||
#ifdef LCD_TELEMETRY
|
|||
case 'A': // button A press
|
|||
case '1': |
|||
if (telemetry == 1) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 1; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
case 'B': // button B press
|
|||
case '2': |
|||
if (telemetry == 2) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 2; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
case 'C': // button C press
|
|||
case '3': |
|||
if (telemetry == 3) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 3; |
|||
LCDclear(); |
|||
#if defined(LOG_VALUES) && defined(DEBUG)
|
|||
cycleTimeMax = 0; // reset min/max on transition on->off
|
|||
cycleTimeMin = 65535; |
|||
#endif
|
|||
} |
|||
break; |
|||
case 'D': // button D press
|
|||
case '4': |
|||
if (telemetry == 4) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 4; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
case '5': |
|||
if (telemetry == 5) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 5; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
case '6': |
|||
if (telemetry == 6) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 6; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
case '7': |
|||
if (telemetry == 7) |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 7; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
#if defined(LOG_VALUES) && defined(DEBUG)
|
|||
case 'R': |
|||
//Reset logvalues
|
|||
if (telemetry == 'R') |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 'R'; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
#endif
|
|||
#ifdef DEBUG
|
|||
case 'F': |
|||
{ |
|||
if (telemetry == 'F') |
|||
telemetry = 0; |
|||
else { |
|||
telemetry = 'F'; |
|||
LCDclear(); |
|||
} |
|||
break; |
|||
} |
|||
#endif
|
|||
case 'a': // button A release
|
|||
case 'b': // button B release
|
|||
case 'c': // button C release
|
|||
case 'd': // button D release
|
|||
break; |
|||
#endif
|
|||
case 'M': // Multiwii @ arduino to GUI all data
|
|||
serialize8('M'); |
|||
serialize8(VERSION); // MultiWii Firmware version
|
|||
for (i = 0; i < 3; i++) |
|||
serialize16(accSmooth[i]); |
|||
for (i = 0; i < 3; i++) |
|||
serialize16(gyroData[i]); |
|||
for (i = 0; i < 3; i++) |
|||
serialize16(magADC[i]); |
|||
serialize16(EstAlt); |
|||
serialize16(heading); // compass
|
|||
for (i = 0; i < 8; i++) |
|||
serialize16(servo[i]); |
|||
for (i = 0; i < 8; i++) |
|||
serialize16(motor[i]); |
|||
for (i = 0; i < 8; i++) |
|||
serialize16(rcData[i]); |
|||
serialize8(nunchuk | ACC << 1 | BARO << 2 | MAG << 3 | GPSPRESENT << 4); |
|||
serialize8(accMode | baroMode << 1 | magMode << 2 | (GPSModeHome | GPSModeHold) << 3); |
|||
#if defined(LOG_VALUES)
|
|||
serialize16(cycleTimeMax); |
|||
cycleTimeMax = 0; |
|||
#else
|
|||
serialize16(cycleTime); |
|||
#endif
|
|||
for (i = 0; i < 2; i++) |
|||
serialize16(angle[i]); |
|||
serialize8(MULTITYPE); |
|||
for (i = 0; i < PIDITEMS; i++) { |
|||
serialize8(P8[i]); |
|||
serialize8(I8[i]); |
|||
serialize8(D8[i]); |
|||
} |
|||
serialize8(rcRate8); |
|||
serialize8(rcExpo8); |
|||
serialize8(rollPitchRate); |
|||
serialize8(yawRate); |
|||
serialize8(dynThrPID); |
|||
for (i = 0; i < CHECKBOXITEMS; i++) { |
|||
serialize8(activate1[i]); |
|||
serialize8(activate2[i]); |
|||
} |
|||
serialize16(GPS_distanceToHome); |
|||
serialize16(GPS_directionToHome); |
|||
serialize8(GPS_numSat); |
|||
serialize8(GPS_fix); |
|||
serialize8(GPS_update); |
|||
serialize16(intPowerMeterSum); |
|||
serialize16(intPowerTrigger1); |
|||
serialize8(vbat); |
|||
serialize16(BaroAlt); // 4 variables are here for general monitoring purpose
|
|||
serialize16(i2c_errors_count); // debug2
|
|||
serialize16(debug3); // debug3
|
|||
serialize16(debug4); // debug4
|
|||
serialize8('M'); |
|||
UartSendData(); |
|||
break; |
|||
case 'O': // arduino to OSD data - contribution from MIS
|
|||
serialize8('O'); |
|||
for (i = 0; i < 3; i++) |
|||
serialize16(accSmooth[i]); |
|||
for (i = 0; i < 3; i++) |
|||
serialize16(gyroData[i]); |
|||
serialize16(EstAlt * 10.0f); |
|||
serialize16(heading); // compass - 16 bytes
|
|||
for (i = 0; i < 2; i++) |
|||
serialize16(angle[i]); //20
|
|||
for (i = 0; i < 6; i++) |
|||
serialize16(motor[i]); //32
|
|||
for (i = 0; i < 6; i++) { |
|||
serialize16(rcData[i]); |
|||
} //44
|
|||
serialize8(nunchuk | ACC << 1 | BARO << 2 | MAG << 3); |
|||
serialize8(accMode | baroMode << 1 | magMode << 2); |
|||
serialize8(vbat); // Vbatt 47
|
|||
serialize8(VERSION); // MultiWii Firmware version
|
|||
serialize8('O'); //49
|
|||
UartSendData(); |
|||
break; |
|||
case 'W': //GUI write params to eeprom @ arduino
|
|||
while (SerialAvailable(0) < (7 + 3 * PIDITEMS + 2 * CHECKBOXITEMS)) { |
|||
} |
|||
for (i = 0; i < PIDITEMS; i++) { |
|||
P8[i] = SerialRead(0); |
|||
I8[i] = SerialRead(0); |
|||
D8[i] = SerialRead(0); |
|||
} |
|||
rcRate8 = SerialRead(0); |
|||
rcExpo8 = SerialRead(0); //2
|
|||
rollPitchRate = SerialRead(0); |
|||
yawRate = SerialRead(0); //4
|
|||
dynThrPID = SerialRead(0); //5
|
|||
for (i = 0; i < CHECKBOXITEMS; i++) { |
|||
activate1[i] = SerialRead(0); |
|||
activate2[i] = SerialRead(0); |
|||
} |
|||
#if defined(POWERMETER)
|
|||
powerTrigger1 = (SerialRead(0) + 256 * SerialRead(0)) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value
|
|||
#else
|
|||
SerialRead(0); |
|||
SerialRead(0); //7 so we unload the two bytes
|
|||
#endif
|
|||
writeParams(); |
|||
break; |
|||
case 'S': //GUI to arduino ACC calibration request
|
|||
calibratingA = 400; |
|||
break; |
|||
case 'E': //GUI to arduino MAG calibration request
|
|||
calibratingM = 1; |
|||
break; |
|||
} |
|||
} |
|||
} |
|||
|
|||
#define SERIAL_RX_BUFFER_SIZE 64
|
|||
|
|||
#if defined(PROMINI)
|
|||
uint8_t serialBufferRX[SERIAL_RX_BUFFER_SIZE][1]; |
|||
volatile uint8_t serialHeadRX[1], serialTailRX[1]; |
|||
#endif
|
|||
#if defined(MEGA)
|
|||
uint8_t serialBufferRX[SERIAL_RX_BUFFER_SIZE][4]; |
|||
volatile uint8_t serialHeadRX[4], serialTailRX[4]; |
|||
#endif
|
|||
|
|||
//#if defined(PROMINI)
|
|||
//uint8_t serialBufferRX0[SERIAL_RX_BUFFER_SIZE];
|
|||
//volatile uint8_t serialHeadRX0,serialTailRX0;
|
|||
//#endif
|
|||
//#if defined(MEGA)
|
|||
//uint8_t serialBufferRX1[SERIAL_RX_BUFFER_SIZE];
|
|||
//volatile uint8_t serialHeadRX1,serialTailRX1;
|
|||
//uint8_t serialBufferRX2[SERIAL_RX_BUFFER_SIZE];
|
|||
//volatile uint8_t serialHeadRX2,serialTailRX2;
|
|||
//uint8_t serialBufferRX3[SERIAL_RX_BUFFER_SIZE];
|
|||
//volatile uint8_t serialHeadRX3,serialTailRX3;
|
|||
//#endif
|
|||
|
|||
void SerialOpen(uint8_t port, uint32_t baud) |
|||
{ |
|||
uint8_t h = ((F_CPU / 4 / baud - 1) / 2) >> 8; |
|||
uint8_t l = ((F_CPU / 4 / baud - 1) / 2); |
|||
switch (port) { |
|||
case 0: |
|||
UCSR0A = (1 << U2X0); |
|||
UBRR0H = h; |
|||
UBRR0L = l; |
|||
UCSR0B |= (1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0); |
|||
break; |
|||
#if defined(MEGA)
|
|||
case 1: |
|||
UCSR1A = (1 << U2X1); |
|||
UBRR1H = h; |
|||
UBRR1L = l; |
|||
UCSR1B |= (1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1); |
|||
break; |
|||
case 2: |
|||
UCSR2A = (1 << U2X2); |
|||
UBRR2H = h; |
|||
UBRR2L = l; |
|||
UCSR2B |= (1 << RXEN2) | (1 << TXEN2) | (1 << RXCIE2); |
|||
break; |
|||
case 3: |
|||
UCSR3A = (1 << U2X3); |
|||
UBRR3H = h; |
|||
UBRR3L = l; |
|||
UCSR3B |= (1 << RXEN3) | (1 << TXEN3) | (1 << RXCIE3); |
|||
break; |
|||
#endif
|
|||
} |
|||
} |
|||
|
|||
void SerialEnd(uint8_t port) |
|||
{ |
|||
switch (port) { |
|||
case 0: |
|||
UCSR0B &= ~((1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0) | (1 << UDRIE0)); |
|||
break; |
|||
#if defined(MEGA)
|
|||
case 1: |
|||
UCSR1B &= ~((1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1)); |
|||
break; |
|||
case 2: |
|||
UCSR2B &= ~((1 << RXEN2) | (1 << TXEN2) | (1 << RXCIE2)); |
|||
break; |
|||
case 3: |
|||
UCSR3B &= ~((1 << RXEN3) | (1 << TXEN3) | (1 << RXCIE3)); |
|||
break; |
|||
#endif
|
|||
} |
|||
} |
|||
|
|||
#if defined(PROMINI) && !(defined(SPEKTRUM))
|
|||
SIGNAL(USART_RX_vect) |
|||
{ |
|||
uint8_t d = UDR0; |
|||
uint8_t i = (serialHeadRX[0] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
if (i != serialTailRX[0]) { |
|||
serialBufferRX[serialHeadRX[0]][0] = d; |
|||
serialHeadRX[0] = i; |
|||
} |
|||
} |
|||
#endif
|
|||
#if defined(MEGA)
|
|||
SIGNAL(USART0_RX_vect) |
|||
{ |
|||
uint8_t d = UDR0; |
|||
uint8_t i = (serialHeadRX[0] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
if (i != serialTailRX[0]) { |
|||
serialBufferRX[serialHeadRX[0]][0] = d; |
|||
serialHeadRX[0] = i; |
|||
} |
|||
} |
|||
|
|||
#if !(defined(SPEKTRUM))
|
|||
SIGNAL(USART1_RX_vect) |
|||
{ |
|||
uint8_t d = UDR1; |
|||
uint8_t i = (serialHeadRX[1] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
if (i != serialTailRX[1]) { |
|||
serialBufferRX[serialHeadRX[1]][1] = d; |
|||
serialHeadRX[1] = i; |
|||
} |
|||
} |
|||
#endif
|
|||
SIGNAL(USART2_RX_vect) |
|||
{ |
|||
uint8_t d = UDR2; |
|||
uint8_t i = (serialHeadRX[2] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
if (i != serialTailRX[2]) { |
|||
serialBufferRX[serialHeadRX[2]][2] = d; |
|||
serialHeadRX[2] = i; |
|||
} |
|||
} |
|||
|
|||
SIGNAL(USART3_RX_vect) |
|||
{ |
|||
uint8_t d = UDR3; |
|||
uint8_t i = (serialHeadRX[3] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
if (i != serialTailRX[3]) { |
|||
serialBufferRX[serialHeadRX[3]][3] = d; |
|||
serialHeadRX[3] = i; |
|||
} |
|||
} |
|||
#endif
|
|||
|
|||
//#if defined(PROMINI) && !(defined(SPEKTRUM))
|
|||
//SIGNAL(USART_RX_vect){
|
|||
// uint8_t d = UDR0;
|
|||
// uint8_t i = (serialHeadRX0 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// if (i != serialTailRX0) {serialBufferRX0[serialHeadRX0] = d; serialHeadRX0 = i;}
|
|||
//}
|
|||
//#endif
|
|||
//#if defined(MEGA)
|
|||
//SIGNAL(USART0_RX_vect){
|
|||
// uint8_t d = UDR0;
|
|||
// uint8_t i = (serialHeadRX0 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// if (i != serialTailRX0) {serialBufferRX0[serialHeadRX0] = d; serialHeadRX0 = i;}
|
|||
//}
|
|||
//#if !(defined(SPEKTRUM))
|
|||
//SIGNAL(USART1_RX_vect){
|
|||
// uint8_t d = UDR1;
|
|||
// uint8_t i = (serialHeadRX1 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// if (i != serialTailRX1) {serialBufferRX1[serialHeadRX1] = d; serialHeadRX1 = i;}
|
|||
//}
|
|||
//#endif
|
|||
//SIGNAL(USART2_RX_vect){
|
|||
// uint8_t d = UDR2;
|
|||
// uint8_t i = (serialHeadRX2 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// if (i != serialTailRX2) {serialBufferRX2[serialHeadRX2] = d; serialHeadRX2 = i;}
|
|||
//}
|
|||
//SIGNAL(USART3_RX_vect){
|
|||
// uint8_t d = UDR3;
|
|||
// uint8_t i = (serialHeadRX3 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// if (i != serialTailRX3) {serialBufferRX3[serialHeadRX3] = d; serialHeadRX3 = i;}
|
|||
//}
|
|||
//#endif
|
|||
|
|||
uint8_t SerialRead(uint8_t port) |
|||
{ |
|||
uint8_t c = serialBufferRX[serialTailRX[port]][port]; |
|||
if ((serialHeadRX[port] != serialTailRX[port])) |
|||
serialTailRX[port] = (serialTailRX[port] + 1) % SERIAL_RX_BUFFER_SIZE; |
|||
return c; |
|||
} |
|||
|
|||
//void SerialRead(uint8_t port,uint8_t c){
|
|||
// switch (port) {
|
|||
// case 0:
|
|||
// uint8_t c = serialBufferRX0[serialTailRX0];
|
|||
// if ((serialHeadRX0 != serialTailRX0)) serialTailRX0 = (serialTailRX0 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// break;
|
|||
// #if defined(MEGA)
|
|||
// case 1:
|
|||
// uint8_t c = serialBufferRX1[serialTailRX1];
|
|||
// if ((serialHeadRX1 != serialTailRX1)) serialTailRX1 = (serialTailRX1 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// break;
|
|||
// case 2:
|
|||
// uint8_t c = serialBufferRX2[serialTailRX2];
|
|||
// if ((serialHeadRX2 != serialTailRX2)) serialTailRX2 = (serialTailRX2 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// break;
|
|||
// case 3:
|
|||
// uint8_t c = serialBufferRX3[serialTailRX3];
|
|||
// if ((serialHeadRX3 != serialTailRX3)) serialTailRX3 = (serialTailRX3 + 1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// break;
|
|||
// #endif
|
|||
// }
|
|||
// return c;
|
|||
//}
|
|||
|
|||
uint8_t SerialAvailable(uint8_t port) |
|||
{ |
|||
return (SERIAL_RX_BUFFER_SIZE + serialHeadRX[port] - serialTailRX[port]) % SERIAL_RX_BUFFER_SIZE; |
|||
} |
|||
|
|||
//void SerialAvailable(uint8_t port,uint8_t c){
|
|||
// switch (port) {
|
|||
// case 0: return (SERIAL_RX_BUFFER_SIZE + serialHeadRX0 - serialTailRX0) % SERIAL_RX_BUFFER_SIZE;
|
|||
// #if defined(MEGA)
|
|||
// case 1: return (SERIAL_RX_BUFFER_SIZE + serialHeadRX1 - serialTailRX1) % SERIAL_RX_BUFFER_SIZE;
|
|||
// case 2: return (SERIAL_RX_BUFFER_SIZE + serialHeadRX2 - serialTailRX2) % SERIAL_RX_BUFFER_SIZE;
|
|||
// case 3: return (SERIAL_RX_BUFFER_SIZE + serialHeadRX3 - serialTailRX3) % SERIAL_RX_BUFFER_SIZE;
|
|||
// #endif
|
|||
// }
|
|||
//}
|
|||
|
|||
void SerialWrite(uint8_t port, uint8_t c) |
|||
{ |
|||
switch (port) { |
|||
case 0: |
|||
serialize8(c); |
|||
UartSendData(); |
|||
break; // Serial0 TX is driven via a buffer and a background intterupt
|
|||
#if defined(MEGA)
|
|||
case 1: |
|||
while (!(UCSR1A & (1 << UDRE1))); |
|||
UDR1 = c; |
|||
break; // Serial1 Serial2 and Serial3 TX are not driven via interrupts
|
|||
case 2: |
|||
while (!(UCSR2A & (1 << UDRE2))); |
|||
UDR2 = c; |
|||
break; |
|||
case 3: |
|||
while (!(UCSR3A & (1 << UDRE3))); |
|||
UDR3 = c; |
|||
break; |
|||
#endif
|
|||
} |
|||
} |
|||
@ -1,427 +0,0 @@ |
|||
/*******************************/ |
|||
/****CONFIGURABLE PARAMETERS****/ |
|||
/*******************************/ |
|||
|
|||
/* 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 */ |
|||
//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A |
|||
//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A |
|||
//#define MINTHROTTLE 1220 |
|||
#define MINTHROTTLE 1150 |
|||
|
|||
/* The type of multicopter */ |
|||
//#define GIMBAL |
|||
//#define BI |
|||
//#define TRI |
|||
//#define QUADP |
|||
#define QUADX |
|||
//#define Y4 |
|||
//#define Y6 |
|||
//#define HEX6 |
|||
//#define HEX6X |
|||
//#define OCTOX8 |
|||
//#define OCTOFLATP |
|||
//#define OCTOFLATX |
|||
//#define FLYING_WING //experimental |
|||
|
|||
#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction |
|||
//#define YAW_DIRECTION -1 |
|||
|
|||
#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP |
|||
//#define I2C_SPEED 400000L //400kHz fast mode, it works only with some WMP clones |
|||
|
|||
//enable internal I2C pull ups |
|||
#define INTERNAL_I2C_PULLUPS |
|||
|
|||
|
|||
//****** advanced users settings ************* |
|||
/* I2C DFRobot LED RING communication */ |
|||
//#define LED_RING |
|||
|
|||
/* This option should be uncommented if ACC Z is accurate enough when motors are running*/ |
|||
/* should now be ok with BMA020 and BMA180 ACC */ |
|||
#define TRUSTED_ACCZ |
|||
|
|||
/* This will activate the ACC-Inflight calibration if unchecked */ |
|||
//#define InflightAccCalibration |
|||
|
|||
/* PIN A0 and A1 instead of PIN D5 & D6 for 6 motors config and promini config |
|||
This mod allow the use of a standard receiver on a pro mini |
|||
(no need to use a PPM sum receiver) |
|||
*/ |
|||
//#define A0_A1_PIN_HEX |
|||
|
|||
/* possibility to use PIN8 or PIN12 as the AUX2 RC input |
|||
it deactivates in this case the POWER PIN (pin 12) or the BUZZER PIN (pin 8) |
|||
*/ |
|||
//#define RCAUXPIN8 |
|||
//#define RCAUXPIN12 |
|||
|
|||
/* This option is here if you want to use the old level code from the verison 1.7 |
|||
It's just to have some feedback. This will be removed in the future */ |
|||
//#define STAB_OLD_17 |
|||
|
|||
/* GPS |
|||
only available on MEGA boards (this might be possible on 328 based boards in the future) |
|||
if enabled, define here the Arduino Serial port number and the UART speed |
|||
note: only the RX PIN is used, the GPS is not configured by multiwii |
|||
the GPS must be configured to output NMEA sentences (which is generally the default conf for most GPS devices) |
|||
*/ |
|||
//#define GPS |
|||
#define GPS_SERIAL 2 // should be 2 for flyduino v2. It's the serial port number on arduino MEGA |
|||
#define GPS_BAUD 115200 |
|||
//#define GPS_BAUD 9600 |
|||
|
|||
/* Pseudo-derivative conrtroller for level mode (experimental) |
|||
Additional information: http://www.multiwii.com/forum/viewtopic.php?f=8&t=503 */ |
|||
//#define LEVEL_PDF |
|||
|
|||
/* introduce a deadband around the stick center |
|||
Must be greater than zero, comment if you dont want a deadband on roll, pitch and yaw */ |
|||
//#define DEADBAND 6 |
|||
|
|||
/* if you use a specific sensor board: |
|||
please submit any correction to this list. |
|||
Note from Alex: I only own some boards |
|||
for other boards, I'm not sure, the info was gathered via rc forums, be cautious */ |
|||
//#define FFIMUv1 // first 9DOF+baro board from Jussi, with HMC5843 <- confirmed by Alex |
|||
//#define FFIMUv2 // second version of 9DOF+baro board from Jussi, with HMC5883 <- confirmed by Alex |
|||
//#define FREEIMUv1 // v0.1 & v0.2 & v0.3 version of 9DOF board from Fabio |
|||
//#define FREEIMUv03 // FreeIMU v0.3 and v0.3.1 |
|||
//#define FREEIMUv035 // FreeIMU v0.3.5 no baro |
|||
//#define FREEIMUv035_MS // FreeIMU v0.3.5_MS <- confirmed by Alex |
|||
//#define FREEIMUv035_BMP // FreeIMU v0.3.5_BMP |
|||
//#define FREEIMUv04 // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA <- confirmed by Alex |
|||
//#define PIPO // 9DOF board from erazz |
|||
//#define QUADRINO // full FC board 9DOF+baro board from witespy with BMP085 baro <- confirmed by Alex |
|||
//#define QUADRINO_ZOOM // full FC board 9DOF+baro board from witespy second edition <- confirmed by Alex |
|||
//#define ALLINONE // full FC board or standalone 9DOF+baro board from CSG_EU |
|||
//#define AEROQUADSHIELDv2 |
|||
//#define ATAVRSBIN1 // Atmel 9DOF (Contribution by EOSBandi). requires 3.3V power. |
|||
//#define SIRIUS // Sirius Navigator IMU <- confirmed by Alex |
|||
//#define SIRIUS600 // Sirius Navigator IMU using the WMP for the gyro |
|||
//#define MINIWII // Jussi's MiniWii Flight Controller |
|||
//#define CITRUSv1_0 // CITRUSv1 from qcrc.ca |
|||
//#define DROTEK_IMU10DOF |
|||
|
|||
|
|||
//if you use independent sensors |
|||
//leave it commented it you already checked a specific board above |
|||
/* I2C gyroscope */ |
|||
//#define ITG3200 |
|||
//#define L3G4200D |
|||
|
|||
/* I2C accelerometer */ |
|||
//#define ADXL345 |
|||
//#define BMA020 |
|||
//#define BMA180 |
|||
//#define NUNCHACK // if you want to use the nunckuk as a standalone I2C ACC without WMP |
|||
//#define LIS3LV02 |
|||
//#define LSM303DLx_ACC |
|||
|
|||
/* I2C barometer */ |
|||
//#define BMP085 |
|||
//#define MS561101BA |
|||
|
|||
/* I2C magnetometer */ |
|||
//#define HMC5843 |
|||
//#define HMC5883 |
|||
//#define AK8975 |
|||
|
|||
/* ADC accelerometer */ // for 5DOF from sparkfun, uses analog PIN A1/A2/A3 |
|||
//#define ADCACC |
|||
|
|||
/* ITG3200 & ITG3205 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try |
|||
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting. |
|||
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and |
|||
balancing options ran out. Uncomment only one option! |
|||
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/ |
|||
//#define ITG3200_LPF_256HZ // This is the default setting, no need to uncomment, just for reference |
|||
//#define ITG3200_LPF_188HZ |
|||
//#define ITG3200_LPF_98HZ |
|||
//#define ITG3200_LPF_42HZ |
|||
//#define ITG3200_LPF_20HZ |
|||
//#define ITG3200_LPF_10HZ // Use this only in extreme cases, rather change motors and/or props |
|||
|
|||
/* MPU6050 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try |
|||
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting. |
|||
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and |
|||
balancing options ran out. Uncomment only one option! |
|||
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/ |
|||
//#define MPU6050_LPF_256HZ // This is the default setting, no need to uncomment, just for reference |
|||
//#define MPU6050_LPF_188HZ |
|||
//#define MPU6050_LPF_98HZ |
|||
//#define MPU6050_LPF_42HZ |
|||
//#define MPU6050_LPF_20HZ |
|||
//#define MPU6050_LPF_10HZ // Use this only in extreme cases, rather change motors and/or props |
|||
|
|||
/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2 |
|||
IF YOUR RECEIVER IS NOT CONCERNED, DON'T UNCOMMENT ANYTHING. Note this is mandatory for a Y6 setup on a promini |
|||
Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */ |
|||
//#define SERIAL_SUM_PPM PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4 //For Graupner/Spektrum |
|||
//#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For Robe/Hitec/Futaba |
|||
//#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For some Hitec/Sanwa/Others |
|||
|
|||
/* The following lines apply only for Spektrum Satellite Receiver |
|||
Spektrum Satellites are 3V devices. DO NOT connect to 5V! |
|||
For MEGA boards, attach sat grey wire to RX1, pin 19. Sat black wire to ground. Sat orange wire to Mega board's 3.3V (or any other 3V to 3.3V source). |
|||
For PROMINI, attach sat grey to RX0. Attach sat black to ground. |
|||
There is no 3.3V source on a pro mini; you can either use a different 3V source, or attach orange to 5V with a 3V regulator in-line (such as http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=MCP1700-3002E/TO-ND) |
|||
If you use an inline-regulator, a standard 3-pin servo connector can connect to ground, +5V, and RX0; solder the correct wires (and the 3V regulator!) to a Spektrum baseRX-to-Sat cable that has been cut in half. |
|||
NOTE: Because there is only one serial port on the Pro Mini, using a Spektrum Satellite implies you CANNOT use the PC based configuration tool. Further, you cannot use on-aircraft serial LCD as the baud rates are incompatible. You can configure by one of two methods: |
|||
1) Use an on-aircraft i2c LCD (such as Eagle Tree or LCD03) for setting gains, reading sensors, etc. |
|||
2) Available now: Comment out the Spektrum definition, upload, plug in PC, configure; uncomment the Spektrum definition, upload, plug in RX, and fly. Repeat as required to configure. |
|||
(Contribution by Danal) */ |
|||
//#define SPEKTRUM 1024 |
|||
//#define SPEKTRUM 2048 |
|||
|
|||
|
|||
/* EXPERIMENTAL !! |
|||
contribution from Captain IxI and Zaggo |
|||
cf http://www.multiwii.com/forum/viewtopic.php?f=7&t=289 |
|||
The following line apply only for Futaba S-Bus Receiver on MEGA boards at RX1 only (Serial 1). |
|||
You have to invert the S-Bus-Serial Signal e.g. with a Hex-Inverter like IC SN74 LS 04 */ |
|||
//#define SBUS |
|||
|
|||
/* Failsave settings - added by MIS |
|||
Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated. |
|||
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered |
|||
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results. |
|||
This value is depended from your configuration, AUW and some other params. |
|||
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped. |
|||
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal. |
|||
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/ |
|||
#define FAILSAFE // Alex: comment this line if you want to deactivate the failsafe function |
|||
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example |
|||
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example |
|||
#define FAILSAVE_THR0TTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case |
|||
|
|||
/* EXPERIMENTAL !! |
|||
contribution from Luis Correia |
|||
see http://www.multiwii.com/forum/viewtopic.php?f=18&t=828 |
|||
It uses a Bluetooth Serial module as the input for controlling the device via an Android application |
|||
As with the SPEKTRUM option, is not possible to use the configuration tool on a mini or promini. */ |
|||
//#define BTSERIAL |
|||
|
|||
/* The following lines apply only for a pitch/roll tilt stabilization system |
|||
On promini board, it is not compatible with config with 6 motors or more |
|||
Uncomment the first line to activate it */ |
|||
//#define SERVO_TILT |
|||
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020 |
|||
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000 |
|||
#define TILT_PITCH_MIDDLE 1500 //servo neutral value |
|||
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement |
|||
#define TILT_ROLL_MIN 1020 |
|||
#define TILT_ROLL_MAX 2000 |
|||
#define TILT_ROLL_MIDDLE 1500 |
|||
#define TILT_ROLL_PROP 10 |
|||
|
|||
/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config |
|||
if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000 |
|||
it is relevent only for a conf with NK */ |
|||
#define INTERLEAVING_DELAY 3000 |
|||
|
|||
/* for V BAT monitoring |
|||
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN |
|||
with R1=33k and R2=51k |
|||
vbat = [0;1023]*16/VBATSCALE */ |
|||
#define VBAT // comment this line to suppress the vbat code |
|||
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage |
|||
#define VBATLEVEL1_3S 107 // 10,7V |
|||
#define VBATLEVEL2_3S 103 // 10,3V |
|||
#define VBATLEVEL3_3S 99 // 9.9V |
|||
#define NO_VBAT 16 // Avoid beeping without any battery |
|||
|
|||
/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds |
|||
it is relevent only for a conf with at least a WMP */ |
|||
#define NEUTRALIZE_DELAY 100000 |
|||
|
|||
/* 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 */ |
|||
#define MINCOMMAND 1000 |
|||
|
|||
/* this is the maximum value for the ESCs at full power |
|||
this value can be increased up to 2000 */ |
|||
#define MAXTHROTTLE 1850 |
|||
|
|||
/* This is the speed of the serial interface. 115200 kbit/s is the best option for a USB connection.*/ |
|||
#define SERIAL_COM_SPEED 115200 |
|||
|
|||
/* In order to save space, it's possibile to desactivate the LCD configuration functions |
|||
comment this line only if you don't plan to used a LCD */ |
|||
#define LCD_CONF |
|||
/* to include setting the aux switches for AUX1 and AUX2 via LCD */ |
|||
//#define LCD_CONF_AUX_12 |
|||
/* to include setting the aux switches for AUX1, AUX2, AUX3 and AUX4 via LCD */ |
|||
//#define LCD_CONF_AUX_1234 |
|||
/* Use this to trigger LCD configuration without a TX - only for debugging - do NOT fly with this activated */ |
|||
//#define LCD_CONF_DEBUG |
|||
|
|||
/* choice of LCD attached for configuration and telemetry, see notes below */ |
|||
#define LCD_SERIAL3W // Alex' initial variant with 3 wires, using rx-pin for transmission @9600 baud fixed |
|||
/* serial (wired or wireless via BT etc.) */ |
|||
//#define LCD_TEXTSTAR // Cat's Whisker LCD_TEXTSTAR Module CW-LCD-02 (Which has 4 input keys for selecting menus) |
|||
//#define LCD_VT100 // vt100 compatible terminal emulation (blueterm, putty, etc.) |
|||
/* i2c devices */ |
|||
//#define LCD_ETPP // Eagle Tree Power Panel LCD, which is i2c (not serial) |
|||
//#define LCD_LCD03 // LCD03, which is i2c |
|||
|
|||
/* keys to navigate the LCD menu (preset to LCD_TEXTSTAR key-depress codes)*/ |
|||
#define LCD_MENU_PREV 'a' |
|||
#define LCD_MENU_NEXT 'c' |
|||
#define LCD_VALUE_UP 'd' |
|||
#define LCD_VALUE_DOWN 'b' |
|||
|
|||
/* To use an LCD03 for configuration: |
|||
http://www.robot-electronics.co.uk/htm/Lcd03tech.htm |
|||
Remove the jumper on its back to set i2c control. |
|||
VCC to +5V VCC (pin1 from top) |
|||
SDA - Pin A4 Mini Pro - Pin 20 Mega (pin2 from top) |
|||
SCL - Pin A5 Mini Pro - Pin 21 Mega (pin3 from top) |
|||
GND to Ground (pin4 from top) |
|||
(by Th0rsten) */ |
|||
|
|||
/* To use an Eagle Tree Power Panel LCD for configuration: |
|||
White wire to Ground |
|||
Red wire to +5V VCC (or to the WMP power pin, if you prefer to reset everything on the bus when WMP resets) |
|||
Yellow wire to SDA - Pin A4 Mini Pro - Pin 20 Mega |
|||
Brown wire to SCL - Pin A5 Mini Pro - Pin 21 Mega |
|||
(Contribution by Danal) */ |
|||
|
|||
/* Cat's whisker LCD_TEXTSTAR LCD |
|||
Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD ) |
|||
Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode |
|||
NO rx / tx line reconfiguration, use natural pins */ |
|||
|
|||
|
|||
/* motors will not spin when the throttle command is in low position |
|||
this is an alternative method to stop immediately the motors */ |
|||
//#define MOTOR_STOP |
|||
|
|||
/* some radios have not a neutral point centered on 1500. can be changed here */ |
|||
#define MIDRC 1500 |
|||
|
|||
/* experimental |
|||
camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */ |
|||
//#define CAMTRIG |
|||
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo |
|||
#define CAM_SERVO_LOW 1020 // the position of LOW state servo |
|||
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms |
|||
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms |
|||
|
|||
/* you can change the tricopter servo travel here */ |
|||
#define TRI_YAW_CONSTRAINT_MIN 1020 |
|||
#define TRI_YAW_CONSTRAINT_MAX 2000 |
|||
#define TRI_YAW_MIDDLE 1500 // tail servo center pos. - use this for initial trim; later trim midpoint via LCD |
|||
|
|||
/* Flying Wing: you can change change servo orientation and servo min/max values here */ |
|||
/* valid for all flight modes, even passThrough mode */ |
|||
/* need to setup servo directions here; no need to swap servos amongst channels at rx */ |
|||
#define PITCH_DIRECTION_L 1 // left servo - pitch orientation |
|||
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation) |
|||
#define ROLL_DIRECTION_L 1 // left servo - roll orientation |
|||
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation) |
|||
#define WING_LEFT_MID 1500 // left servo center pos. - use this for initial trim; later trim midpoint via LCD |
|||
#define WING_RIGHT_MID 1500 // right servo center pos. - use this for initial trim; later trim midpoint via LCD |
|||
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000] |
|||
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000] |
|||
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000] |
|||
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000] |
|||
|
|||
/* enable monitoring of the power consumption from battery (think of mAh) */ |
|||
/* allows to set alarm value in GUI or via LCD */ |
|||
/* Two options: */ |
|||
/* 1 - soft: - (good results +-5% for plush and mystery ESCs @ 2S and 3S, not good with SuperSimple ESC */ |
|||
/* 00. relies on your combo of battery type (Voltage, cpacity), ESC, ESC settings, motors, props and multiwii cycle time */ |
|||
/* 01. set POWERMETER soft. Uses PLEVELSCALE = 50, PLEVELDIV = PLEVELDIVSOFT = 5000 */ |
|||
/* 0. output is a value that linearily scales to power (mAh) */ |
|||
/* 1. get voltage reading right first */ |
|||
/* 2. start with freshly charged battery */ |
|||
/* 3. go fly your typical flight (routine and duration) */ |
|||
/* 4. at end connect to GUI or LCD and read the power value; write it down (example 4711)*/ |
|||
/* 5. charge battery, write down amount of energy needed (example 722 mAh) */ |
|||
/* 6. compute alarm value for desired power threshold (example 750 mAh : alarm = 4711 / 722 * 750) */ |
|||
/* 7. set alarm value in GUI or LCD */ |
|||
/* 8. enjoy your new battery alarm - possibly repeat steps 2 .. 7 */ |
|||
/* 9. if you want the numbers to represent your mAh value, you must change PLEVELDIV */ |
|||
/* 2 - hard: - (uses hardware sensor, after configuration gives reasonable results */ |
|||
/* 00. uses analog pin 2 to read voltage output from sensor. */ |
|||
/* 01. set POWERMETER hard. Uses PLEVELSCALE = 50 */ |
|||
/* 02. install low path filter for 25 Hz to sensor input */ |
|||
/* 03. check your average cycle time. If not close to 3ms, then you must change PLEVELDIV accordingly */ |
|||
/* 1. compute PLEVELDIV for your sensor (see below for insturctions) */ |
|||
/* 2. set PLEVELDIVSOFT to 5000 ( to use LOG_VALUES for individual motor comparison) */ |
|||
/* 3. attach, set PSENSORNULL and PINT2mA */ |
|||
/* 4. configure, compile, upload, set alarm value in GUI or LCD */ |
|||
/* 3. enjoy true readings of mAh consumed */ |
|||
/* set POWERMETER to "soft" (1) or "hard" (2) depending on sensor you want to utilize */ |
|||
//#define POWERMETER_SOFT |
|||
//#define POWERMETER_HARD |
|||
/* the sum of all powermeters ranges from [0:60000 e4] theoretically. */ |
|||
/* the alarm level from eeprom is out of [0:255], so we multipy alarm level with PLEVELSCALE and with 1e4 before comparing */ |
|||
/* PLEVELSCALE is the step size you can use to set alarm */ |
|||
#define PLEVELSCALE 50 // if you change this value for other granularity, you must search for comments in code to change accordingly |
|||
/* larger PLEVELDIV will get you smaller value for power (mAh equivalent) */ |
|||
#define PLEVELDIV 5000 // default for soft - if you lower PLEVELDIV, beware of overrun in uint32 pMeter |
|||
#define PLEVELDIVSOFT PLEVELDIV // for soft always equal to PLEVELDIV; for hard set to 5000 |
|||
//#define PLEVELDIV 1361L // to convert the sum into mAh divide by this value |
|||
/* amploc 25A sensor has 37mV/A */ |
|||
/* arduino analog resolution is 4.9mV per unit; units from [0..1023] */ |
|||
/* sampling rate 20ms, approx 19977 micro seconds */ |
|||
/* PLEVELDIV = 37 / 4.9 * 10e6 / 18000 * 3600 / 1000 = 1361L */ |
|||
/* set to analogRead() value for zero current */ |
|||
#define PSENSORNULL 510 // for I=0A my sensor gives 1/2 Vss; that is approx 2.49Volt |
|||
#define PINT2mA 13 // for telemtry display: one integer step on arduino analog translates to mA (example 4.9 / 37 * 100 |
|||
|
|||
|
|||
|
|||
/* to monitor system values (battery level, loop time etc. with LCD enable this */ |
|||
/* note: for now you must send single characters 'A', 'B', 'C', 'D' to request 4 different pages */ |
|||
/* Buttons toggle request for page on/off */ |
|||
/* The active page on the LCD does get updated automatically */ |
|||
/* Easy to use with Terminal application or display like LCD - uses the 4 buttons are preconfigured to send 'A', 'B', 'C', 'D' */ |
|||
//#define LCD_TELEMETRY |
|||
/* to enable automatic hopping between a choice of telemetry pages uncomment this. */ |
|||
/* This may be useful if your LCD has no buttons or the sending is broken */ |
|||
/* hopping is activated and deactivated in unarmed mode with throttle=low & roll=left & pitch=forward */ |
|||
/* set it to the sequence of telemetry pages you want to see */ |
|||
//#define LCD_TELEMETRY_AUTO "12345267" // pages 1 to 7 in ascending order |
|||
//#define LCD_TELEMETRY_AUTO "2122324252627" // strong emphasis on page 2 |
|||
/* Use this to trigger telemetry without a TX - only for debugging - do NOT fly with this activated */ |
|||
//#define LCD_TELEMETRY_DEBUG //This form rolls between all screens, LCD_TELEMETRY_AUTO must also be defined. |
|||
//#define LCD_TELEMETRY_DEBUG 6 //This form stays on the screen specified. |
|||
|
|||
/* on telemetry page B it gives a bar graph which shows how much voltage battery has left. Range from 0 to 12 Volt is not very informative */ |
|||
/* so we try do define a meaningful part. For a 3S battery we define full=12,6V and calculate how much it is above first warning level */ |
|||
/* Example: 12.6V - VBATLEVEL1_3S (for me = 126 - 102 = 24) */ |
|||
#define VBATREF 24 |
|||
|
|||
/* to log values like max loop time and others to come */ |
|||
/* logging values are visible via LCD config */ |
|||
/* set to 2, if you want powerconsumption on a per motor basis (this uses the big array and is a memory hog, if POWERMETER <> PM_SOFT) */ |
|||
//#define LOG_VALUES 1 |
|||
|
|||
/* to add debugging code */ |
|||
/* not needed and not recommended for normal operation */ |
|||
/* will add extra code that may slow down the main loop or make copter non-flyable */ |
|||
//#define DEBUG |
|||
|
|||
//****** end of advanced users settings ************* |
|||
|
|||
//if you want to change to orientation of individual sensor |
|||
//#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z;} |
|||
//#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z;} |
|||
//#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;} |
|||
|
|||
|
|||
/* frequenies for rare cyclic actions in the main loop, depend on cycle time! */ |
|||
/* time base is main loop cycle time - a value of 6 means to trigger the action every 6th run through the main loop */ |
|||
/* example: with cycle time of approx 3ms, do action every 6*3ms=18ms */ |
|||
/* value must be [1; 65535] */ |
|||
#define LCD_TELEMETRY_FREQ 23 // to send telemetry data over serial 23 <=> 60ms <=> 16Hz (only sending interlaced, so 8Hz update rate) |
|||
#define LCD_TELEMETRY_AUTO_FREQ 667 // to step to next telemetry page 667 <=> 2s |
|||
#define PSENSORFREQ 6 // to read hardware powermeter sensor 6 <=> 18ms |
|||
#define VBATFREQ PSENSORFREQ // to read battery voltage - keep equal to PSENSORFREQ unless you know what you are doing |
|||
/**************************************/ |
|||
/****END OF CONFIGURABLE PARAMETERS****/ |
|||
/**************************************/ |
|||
@ -1,484 +0,0 @@ |
|||
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) |
|||
#define PROMINI |
|||
#endif |
|||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) |
|||
#define MEGA |
|||
#endif |
|||
|
|||
#if defined(PROMINI) |
|||
#define LEDPIN_PINMODE pinMode (13, OUTPUT); |
|||
#define LEDPIN_TOGGLE PINB |= 1<<5; //switch LEDPIN state (digital PIN 13) |
|||
#define LEDPIN_OFF PORTB &= ~(1<<5); |
|||
#define LEDPIN_ON PORTB |= (1<<5); |
|||
#define BUZZERPIN_PINMODE pinMode (8, OUTPUT); |
|||
#define BUZZERPIN_ON PORTB |= 1; |
|||
#define BUZZERPIN_OFF PORTB &= ~1; |
|||
#define POWERPIN_PINMODE pinMode (12, OUTPUT); |
|||
#define POWERPIN_ON PORTB |= 1<<4; |
|||
#define POWERPIN_OFF PORTB &= ~(1<<4); //switch OFF WMP, digital PIN 12 |
|||
#define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL) |
|||
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5); |
|||
#define PINMODE_LCD pinMode(0, OUTPUT); |
|||
#define LCDPIN_OFF PORTD &= ~1; |
|||
#define LCDPIN_ON PORTD |= 1; |
|||
#define STABLEPIN_PINMODE ; |
|||
#define STABLEPIN_ON ; |
|||
#define STABLEPIN_OFF ; |
|||
#define DIGITAL_SERVO_TRI_PINMODE pinMode(3,OUTPUT); //also right servo for BI COPTER |
|||
#define DIGITAL_SERVO_TRI_HIGH PORTD |= 1<<3; |
|||
#define DIGITAL_SERVO_TRI_LOW PORTD &= ~(1<<3); |
|||
#define DIGITAL_TILT_PITCH_PINMODE pinMode(A0,OUTPUT); |
|||
#define DIGITAL_TILT_PITCH_HIGH PORTC |= 1<<0; |
|||
#define DIGITAL_TILT_PITCH_LOW PORTC &= ~(1<<0); |
|||
#define DIGITAL_TILT_ROLL_PINMODE pinMode(A1,OUTPUT); |
|||
#define DIGITAL_TILT_ROLL_HIGH PORTC |= 1<<1; |
|||
#define DIGITAL_TILT_ROLL_LOW PORTC &= ~(1<<1); |
|||
#define DIGITAL_BI_LEFT_PINMODE pinMode(11,OUTPUT); |
|||
#define DIGITAL_BI_LEFT_HIGH PORTB |= 1<<3; |
|||
#define DIGITAL_BI_LEFT_LOW PORTB &= ~(1<<3); |
|||
#define PPM_PIN_INTERRUPT attachInterrupt(0, rxInt, RISING); //PIN 0 |
|||
#define SPEK_SERIAL_VECT USART_RX_vect |
|||
#define SPEK_DATA_REG UDR0 |
|||
// #define MOTOR_ORDER 9,10,11,3,6,5 //for a quad+: rear,right,left,front |
|||
#define DIGITAL_CAM_PINMODE pinMode(A2,OUTPUT); |
|||
#define DIGITAL_CAM_HIGH PORTC |= 1<<2; |
|||
#define DIGITAL_CAM_LOW PORTC &= ~(1<<2); |
|||
//RX PIN assignment inside the port //for PORTD |
|||
#define THROTTLEPIN 2 |
|||
#define ROLLPIN 4 |
|||
#define PITCHPIN 5 |
|||
#define YAWPIN 6 |
|||
#define AUX1PIN 7 |
|||
#define AUX2PIN 0 // optional PIN 8 or PIN 12 |
|||
#define AUX3PIN 1 // unused |
|||
#define AUX4PIN 3 // unused |
|||
#define ISR_UART ISR(USART_UDRE_vect) |
|||
#define V_BATPIN A3 // Analog PIN 3 |
|||
#define PSENSORPIN A2 // Analog PIN 2 |
|||
|
|||
//motor order changes because of possible octo |
|||
#define MOTOR_ORDER 9,10,11,3,6,5,A2,12 //for a quad+: rear,right,left,front |
|||
|
|||
// TILT_PITCH |
|||
#define SERVO_1_PINMODE pinMode(A0,OUTPUT); |
|||
#define SERVO_1_PIN_HIGH PORTC |= 1<<0; |
|||
#define SERVO_1_PIN_LOW PORTC &= ~(1<<0); |
|||
|
|||
// TILT_ROLL |
|||
#define SERVO_2_PINMODE pinMode(A1,OUTPUT); |
|||
#define SERVO_2_PIN_HIGH PORTC |= 1<<1; |
|||
#define SERVO_2_PIN_LOW PORTC &= ~(1<<1); |
|||
|
|||
// CAM TRIG |
|||
#define SERVO_3_PINMODE pinMode(A2,OUTPUT); |
|||
#define SERVO_3_PIN_HIGH PORTC |= 1<<2; |
|||
#define SERVO_3_PIN_LOW PORTC &= ~(1<<2); |
|||
|
|||
// new |
|||
#define SERVO_4_PINMODE pinMode(12,OUTPUT); |
|||
#define SERVO_4_PIN_HIGH PORTB |= 1<<4; |
|||
#define SERVO_4_PIN_LOW PORTB &= ~(1<<4); |
|||
|
|||
// BI LEFT |
|||
#define SERVO_5_PINMODE pinMode(3,OUTPUT); |
|||
#define SERVO_5_PIN_HIGH PORTD|= 1<<3; |
|||
#define SERVO_5_PIN_LOW PORTD &= ~(1<<3); |
|||
|
|||
// TRI REAR |
|||
#define SERVO_6_PINMODE pinMode(11,OUTPUT); |
|||
#define SERVO_6_PIN_HIGH PORTB |= 1<<3; |
|||
#define SERVO_6_PIN_LOW PORTB &= ~(1<<3); |
|||
|
|||
// new motor pin 10 |
|||
#define SERVO_7_PINMODE pinMode(10,OUTPUT); |
|||
#define SERVO_7_PIN_HIGH PORTB |= 1<<2; |
|||
#define SERVO_7_PIN_LOW PORTB &= ~(1<<2); |
|||
|
|||
//new motor pin 9 |
|||
#define SERVO_8_PINMODE pinMode(9,OUTPUT); |
|||
#define SERVO_8_PIN_HIGH PORTB |= 1<<1; |
|||
#define SERVO_8_PIN_LOW PORTB &= ~(1<<1); |
|||
|
|||
#endif |
|||
#if defined(MEGA) |
|||
#define LEDPIN_PINMODE pinMode (13, OUTPUT);pinMode (30, OUTPUT); |
|||
#define LEDPIN_TOGGLE PINB |= (1<<7); PINC |= (1<<7); |
|||
#define LEDPIN_ON PORTB |= (1<<7); PORTC |= (1<<7); |
|||
#define LEDPIN_OFF PORTB &= ~(1<<7);PORTC &= ~(1<<7); |
|||
#define BUZZERPIN_PINMODE pinMode (32, OUTPUT); |
|||
#define BUZZERPIN_ON PORTC |= 1<<5; |
|||
#define BUZZERPIN_OFF PORTC &= ~(1<<5); |
|||
#define POWERPIN_PINMODE pinMode (37, OUTPUT); |
|||
#define POWERPIN_ON PORTC |= 1<<0; |
|||
#define POWERPIN_OFF PORTC &= ~(1<<0); |
|||
#define I2C_PULLUPS_ENABLE PORTD |= 1<<0; PORTD |= 1<<1; // PIN 20&21 (SDA&SCL) |
|||
#define I2C_PULLUPS_DISABLE PORTD &= ~(1<<0); PORTD &= ~(1<<1); |
|||
#define PINMODE_LCD pinMode(0, OUTPUT); |
|||
#define LCDPIN_OFF PORTE &= ~1; //switch OFF digital PIN 0 |
|||
#define LCDPIN_ON PORTE |= 1; //switch OFF digital PIN 0 |
|||
#define STABLEPIN_PINMODE pinMode (31, OUTPUT); |
|||
#define STABLEPIN_ON PORTC |= 1<<6; |
|||
#define STABLEPIN_OFF PORTC &= ~(1<<6); |
|||
#define DIGITAL_SERVO_TRI_PINMODE pinMode(2,OUTPUT); //PIN 2 //also right servo for BI COPTER |
|||
#define DIGITAL_SERVO_TRI_HIGH PORTE |= 1<<4; |
|||
#define DIGITAL_SERVO_TRI_LOW PORTE &= ~(1<<4); |
|||
#define DIGITAL_TILT_PITCH_PINMODE pinMode(34,OUTPUT);pinMode(44,OUTPUT); // 34 + 44 |
|||
#define DIGITAL_TILT_PITCH_HIGH PORTC |= 1<<3;PORTL |= 1<<5; |
|||
#define DIGITAL_TILT_PITCH_LOW PORTC &= ~(1<<3);PORTL &= ~(1<<5); |
|||
#define DIGITAL_TILT_ROLL_PINMODE pinMode(35,OUTPUT);pinMode(45,OUTPUT); // 35 + 45 |
|||
#define DIGITAL_TILT_ROLL_HIGH PORTC |= 1<<2;PORTL |= 1<<4; |
|||
#define DIGITAL_TILT_ROLL_LOW PORTC &= ~(1<<2);PORTL &= ~(1<<4); |
|||
#define DIGITAL_BI_LEFT_PINMODE pinMode(6,OUTPUT); |
|||
#define DIGITAL_BI_LEFT_HIGH PORTH |= 1<<3; |
|||
#define DIGITAL_BI_LEFT_LOW PORTH &= ~(1<<3); |
|||
#define PPM_PIN_INTERRUPT attachInterrupt(4, rxInt, RISING); //PIN 19, also used for Spektrum satellite option |
|||
#define SPEK_SERIAL_VECT USART1_RX_vect |
|||
#define SPEK_DATA_REG UDR1 |
|||
#define DIGITAL_CAM_PINMODE pinMode(33,OUTPUT); pinMode(46,OUTPUT); // 33 + 46 |
|||
#define DIGITAL_CAM_HIGH PORTC |= 1<<4;PORTL |= 1<<3; |
|||
#define DIGITAL_CAM_LOW PORTC &= ~(1<<4);PORTL &= ~(1<<3); |
|||
//RX PIN assignment inside the port //for PORTK |
|||
#define THROTTLEPIN 0 //PIN 62 = PIN A8 |
|||
#define ROLLPIN 1 //PIN 63 = PIN A9 |
|||
#define PITCHPIN 2 //PIN 64 = PIN A10 |
|||
#define YAWPIN 3 //PIN 65 = PIN A11 |
|||
#define AUX1PIN 4 //PIN 66 = PIN A12 |
|||
#define AUX2PIN 5 //PIN 67 = PIN A13 |
|||
#define AUX3PIN 6 //PIN 68 = PIN A14 |
|||
#define AUX4PIN 7 //PIN 69 = PIN A15 |
|||
#define ISR_UART ISR(USART0_UDRE_vect) |
|||
#define V_BATPIN A0 // Analog PIN 0 |
|||
#define PSENSORPIN A2 // Analog PIN 2 |
|||
|
|||
#define MOTOR_ORDER 3,5,6,2,7,8,9,10 //for a quad+: rear,right,left,front //+ for y6: 7:under right 8:under left |
|||
|
|||
// TILT_PITCH |
|||
#define SERVO_1_PINMODE pinMode(34,OUTPUT);pinMode(44,OUTPUT); |
|||
#define SERVO_1_PIN_HIGH PORTC |= 1<<3;PORTL |= 1<<5; |
|||
#define SERVO_1_PIN_LOW PORTC &= ~(1<<3);PORTL &= ~(1<<5); |
|||
|
|||
// TILT_ROLL |
|||
#define SERVO_2_PINMODE pinMode(35,OUTPUT);pinMode(45,OUTPUT); |
|||
#define SERVO_2_PIN_HIGH PORTC |= 1<<2;PORTL |= 1<<4; |
|||
#define SERVO_2_PIN_LOW PORTC &= ~(1<<2);PORTL &= ~(1<<4); |
|||
|
|||
// CAM TRIG |
|||
#define SERVO_3_PINMODE pinMode(33,OUTPUT); pinMode(46,OUTPUT); |
|||
#define SERVO_3_PIN_HIGH PORTC |= 1<<4;PORTL |= 1<<3; |
|||
#define SERVO_3_PIN_LOW PORTC &= ~(1<<4);PORTL &= ~(1<<3); |
|||
|
|||
// new ? |
|||
#define SERVO_4_PINMODE pinMode (37, OUTPUT); |
|||
#define SERVO_4_PIN_HIGH PORTC |= 1<<0; |
|||
#define SERVO_4_PIN_LOW PORTC &= ~(1<<0); |
|||
|
|||
// BI LEFT |
|||
#define SERVO_5_PINMODE pinMode(6,OUTPUT); |
|||
#define SERVO_5_PIN_HIGH PORTH |= 1<<3; |
|||
#define SERVO_5_PIN_LOW PORTH &= ~(1<<3); |
|||
|
|||
// TRI REAR |
|||
#define SERVO_6_PINMODE pinMode(2,OUTPUT); |
|||
#define SERVO_6_PIN_HIGH PORTE |= 1<<4; |
|||
#define SERVO_6_PIN_LOW PORTE &= ~(1<<4); |
|||
|
|||
//new motor pin 5 |
|||
#define SERVO_7_PINMODE pinMode(5,OUTPUT); |
|||
#define SERVO_7_PIN_HIGH PORTE |= 1<<3; |
|||
#define SERVO_7_PIN_LOW PORTE &= ~(1<<3); |
|||
|
|||
//new motor pin 3 |
|||
#define SERVO_8_PINMODE pinMode(3,OUTPUT); |
|||
#define SERVO_8_PIN_HIGH PORTE |= 1<<5; |
|||
#define SERVO_8_PIN_LOW PORTE &= ~(1<<5); |
|||
#endif |
|||
|
|||
|
|||
//please submit any correction to this list. |
|||
#if defined(FFIMUv1) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5843 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#endif |
|||
|
|||
#if defined(FFIMUv2) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#endif |
|||
|
|||
#if defined(FREEIMUv1) |
|||
#define ITG3200 |
|||
#define ADXL345 |
|||
#define HMC5843 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = -Y; accADC[PITCH] = X; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;} |
|||
#define ADXL345_ADDRESS 0xA6 |
|||
#undef INTERNAL_I2C_PULLUPS |
|||
#endif |
|||
|
|||
#if defined(FREEIMUv03) |
|||
#define ITG3200 |
|||
#define ADXL345 // this is actually an ADXL346 but that's just the same as ADXL345 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = -Y; accADC[PITCH] = X; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define ADXL345_ADDRESS 0xA6 |
|||
#undef INTERNAL_I2C_PULLUPS |
|||
#endif |
|||
|
|||
#if defined(FREEIMUv035) || defined(FREEIMUv035_MS) || defined(FREEIMUv035_BMP) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#undef INTERNAL_I2C_PULLUPS |
|||
#if defined(FREEIMUv035_MS) |
|||
#define MS561101BA |
|||
#elif defined(FREEIMUv035_BMP) |
|||
#define BMP085 |
|||
#endif |
|||
#endif |
|||
|
|||
#if defined(FREEIMUv04) |
|||
#define MPU6050 |
|||
#define HMC5883 |
|||
#define MS561101BA |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define MPU6050_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 |
|||
#undef INTERNAL_I2C_PULLUPS |
|||
#endif |
|||
|
|||
#if defined(PIPO) |
|||
#define L3G4200D |
|||
#define ADXL345 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = -Y; accADC[PITCH] = X; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;} |
|||
#define ADXL345_ADDRESS 0xA6 |
|||
#endif |
|||
|
|||
#if defined(QUADRINO) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#endif |
|||
|
|||
#if defined(QUADRINO_ZOOM) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 // note, can be also #define MS561101BA on some versions |
|||
//#define MS561101BA |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#define STABLEPIN_PINMODE pinMode (A2, OUTPUT); |
|||
#define STABLEPIN_ON PORTC |= (1<<2); |
|||
#define STABLEPIN_OFF PORTC &= ~(1<<2); |
|||
#endif |
|||
|
|||
#if defined(ALLINONE) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#endif |
|||
|
|||
#if defined(AEROQUADSHIELDv2) // to confirm |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5843 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = -Y; accADC[PITCH] = X; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define ITG3200_ADDRESS 0XD2 |
|||
#endif |
|||
|
|||
#if defined(ATAVRSBIN1) |
|||
#define ITG3200 |
|||
#define BMA020 //Actually it's a BMA150, but this is a drop in replacement for the discountinued BMA020 |
|||
#define AK8975 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -X; magADC[PITCH] = -Y; magADC[YAW] = -Z;} |
|||
#endif |
|||
|
|||
#if defined(SIRIUS) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#endif |
|||
|
|||
#if defined(SIRIUS600) |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define BMA180_ADDRESS 0x80 |
|||
#endif |
|||
|
|||
#if defined(MINIWII) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = -Y; accADC[YAW] = -Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#endif |
|||
|
|||
#if defined(CITRUSv1_0) |
|||
#define ITG3200 |
|||
#define ADXL345 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(Y, X, Z) {accADC[ROLL] = -X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(Y, X, Z) {magADC[ROLL] = Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define ADXL345_ADDRESS 0xA6 |
|||
#define ITG3200_ADDRESS 0XD0 |
|||
#endif |
|||
|
|||
#if defined(DROTEK_IMU10DOF) |
|||
#define ITG3200 |
|||
#define BMA180 |
|||
#define BMP085 |
|||
#define HMC5883 |
|||
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;} |
|||
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;} |
|||
#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = -Y; magADC[PITCH] = X; magADC[YAW] = Z;} |
|||
#define ITG3200_ADDRESS 0XD2 |
|||
#endif |
|||
|
|||
#if defined(ADXL345) || defined(BMA020) || defined(BMA180) || defined(NUNCHACK) || defined(ADCACC) || defined(LSM303DLx_ACC) || defined(MPU6050) |
|||
#define ACC 1 |
|||
#else |
|||
#define ACC 0 |
|||
#endif |
|||
|
|||
#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) |
|||
#define MAG 1 |
|||
#else |
|||
#define MAG 0 |
|||
#endif |
|||
|
|||
#if defined(ITG3200) || defined(L3G4200D) || defined(MPU6050) |
|||
#define GYRO 1 |
|||
#else |
|||
#define GYRO 0 |
|||
#endif |
|||
|
|||
#if defined(BMP085) || defined(MS561101BA) |
|||
#define BARO 1 |
|||
#else |
|||
#define BARO 0 |
|||
#endif |
|||
|
|||
#if defined(GPS) |
|||
#define GPSPRESENT 1 |
|||
#else |
|||
#define GPSPRESENT 0 |
|||
#endif |
|||
|
|||
|
|||
#if defined(RCAUXPIN8) |
|||
#define BUZZERPIN_PINMODE ; |
|||
#define BUZZERPIN_ON ; |
|||
#define BUZZERPIN_OFF ; |
|||
#define RCAUXPIN |
|||
#endif |
|||
#if defined(RCAUXPIN12) |
|||
#define POWERPIN_PINMODE ; |
|||
#define POWERPIN_ON ; |
|||
#define POWERPIN_OFF ; |
|||
#define RCAUXPIN |
|||
#endif |
|||
|
|||
|
|||
#if defined(TRI) |
|||
#define MULTITYPE 1 |
|||
#elif defined(QUADP) |
|||
#define MULTITYPE 2 |
|||
#elif defined(QUADX) |
|||
#define MULTITYPE 3 |
|||
#elif defined(BI) |
|||
#define MULTITYPE 4 |
|||
#elif defined(GIMBAL) |
|||
#define MULTITYPE 5 |
|||
#elif defined(Y6) |
|||
#define MULTITYPE 6 |
|||
#elif defined(HEX6) |
|||
#define MULTITYPE 7 |
|||
#elif defined(FLYING_WING) |
|||
#define MULTITYPE 8 |
|||
#elif defined(Y4) |
|||
#define MULTITYPE 9 |
|||
#elif defined(HEX6X) |
|||
#define MULTITYPE 10 |
|||
#elif defined(OCTOX8) |
|||
#define MULTITYPE 11 |
|||
#elif defined(OCTOFLATP) |
|||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs |
|||
#elif defined(OCTOFLATX) |
|||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs |
|||
#endif |
|||
|
|||
#if defined(POWERMETER_HARD) || defined(POWERMETER_SOFT) |
|||
#define POWERMETER |
|||
#endif |
|||
|
|||
/**************************/ |
|||
/* Error Checking Section */ |
|||
/**************************/ |
|||
|
|||
#if (defined(LCD_CONF) || defined(LCD_TELEMETRY)) && !(defined(LCD_SERIAL3W) || defined(LCD_TEXTSTAR) || defined(LCD_VT100) || defined(LCD_ETPP) || defined(LCD_LCD03)) |
|||
#error "LCD_CONF or LCD_TELEMETRY defined, and choice of LCD not defined. Uncomment one of LCD_SERIAL3W or LCD_TEXTSTAR or LCD_VT100 or LCD_ETPP or LCD_LCD03" |
|||
#endif |
|||
|
|||
|
|||
#if defined(POWERMETER) && !(defined(VBAT)) |
|||
#error "to use powermeter, you must also define and configure VBAT" |
|||
#endif |
|||
|
|||
#if defined(LCD_TELEMETRY_AUTO) && !(defined(LCD_TELEMETRY)) |
|||
#error "to use automatic telemetry, you MUST also define and configure LCD_TELEMETRY" |
|||
#endif |
|||
|
|||
|
|||
|
|||
@ -0,0 +1,150 @@ |
|||
/* |
|||
***************************************************************************** |
|||
** |
|||
** File : stm32_flash.ld |
|||
** |
|||
** Abstract : Linker script for STM32F103C8 Device with |
|||
** 64KByte FLASH, 20KByte RAM |
|||
** |
|||
***************************************************************************** |
|||
*/ |
|||
|
|||
/* Entry Point */ |
|||
ENTRY(Reset_Handler) |
|||
|
|||
/* Highest address of the user mode stack */ |
|||
_estack = 0x20005000; /* end of 10K RAM */ |
|||
|
|||
/* Generate a link error if heap and stack don't fit into RAM */ |
|||
_Min_Heap_Size = 0; /* required amount of heap */ |
|||
_Min_Stack_Size = 0x400; /* required amount of stack */ |
|||
|
|||
/* Specify the memory areas */ |
|||
MEMORY |
|||
{ |
|||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 64K |
|||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K |
|||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K |
|||
} |
|||
|
|||
/* Define output sections */ |
|||
SECTIONS |
|||
{ |
|||
/* The startup code goes first into FLASH */ |
|||
.isr_vector : |
|||
{ |
|||
. = ALIGN(4); |
|||
KEEP(*(.isr_vector)) /* Startup code */ |
|||
. = ALIGN(4); |
|||
} >FLASH |
|||
|
|||
/* The program code and other data goes into FLASH */ |
|||
.text : |
|||
{ |
|||
. = ALIGN(4); |
|||
*(.text) /* .text sections (code) */ |
|||
*(.text*) /* .text* sections (code) */ |
|||
*(.rodata) /* .rodata sections (constants, strings, etc.) */ |
|||
*(.rodata*) /* .rodata* sections (constants, strings, etc.) */ |
|||
*(.glue_7) /* glue arm to thumb code */ |
|||
*(.glue_7t) /* glue thumb to arm code */ |
|||
*(.eh_frame) |
|||
|
|||
KEEP (*(.init)) |
|||
KEEP (*(.fini)) |
|||
|
|||
. = ALIGN(4); |
|||
_etext = .; /* define a global symbols at end of code */ |
|||
} >FLASH |
|||
|
|||
|
|||
.ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH |
|||
.ARM : { |
|||
__exidx_start = .; |
|||
*(.ARM.exidx*) |
|||
__exidx_end = .; |
|||
} >FLASH |
|||
|
|||
.preinit_array : |
|||
{ |
|||
PROVIDE_HIDDEN (__preinit_array_start = .); |
|||
KEEP (*(.preinit_array*)) |
|||
PROVIDE_HIDDEN (__preinit_array_end = .); |
|||
} >FLASH |
|||
.init_array : |
|||
{ |
|||
PROVIDE_HIDDEN (__init_array_start = .); |
|||
KEEP (*(SORT(.init_array.*))) |
|||
KEEP (*(.init_array*)) |
|||
PROVIDE_HIDDEN (__init_array_end = .); |
|||
} >FLASH |
|||
.fini_array : |
|||
{ |
|||
PROVIDE_HIDDEN (__fini_array_start = .); |
|||
KEEP (*(.fini_array*)) |
|||
KEEP (*(SORT(.fini_array.*))) |
|||
PROVIDE_HIDDEN (__fini_array_end = .); |
|||
} >FLASH |
|||
|
|||
/* used by the startup to initialize data */ |
|||
_sidata = .; |
|||
|
|||
/* Initialized data sections goes into RAM, load LMA copy after code */ |
|||
.data : |
|||
{ |
|||
. = ALIGN(4); |
|||
_sdata = .; /* create a global symbol at data start */ |
|||
*(.data) /* .data sections */ |
|||
*(.data*) /* .data* sections */ |
|||
|
|||
. = ALIGN(4); |
|||
_edata = .; /* define a global symbol at data end */ |
|||
} >RAM AT> FLASH |
|||
|
|||
/* Uninitialized data section */ |
|||
. = ALIGN(4); |
|||
.bss : |
|||
{ |
|||
/* This is used by the startup in order to initialize the .bss secion */ |
|||
_sbss = .; /* define a global symbol at bss start */ |
|||
__bss_start__ = _sbss; |
|||
*(.bss) |
|||
*(.bss*) |
|||
*(COMMON) |
|||
|
|||
. = ALIGN(4); |
|||
_ebss = .; /* define a global symbol at bss end */ |
|||
__bss_end__ = _ebss; |
|||
} >RAM |
|||
|
|||
/* User_heap_stack section, used to check that there is enough RAM left */ |
|||
._user_heap_stack : |
|||
{ |
|||
. = ALIGN(4); |
|||
PROVIDE ( end = . ); |
|||
PROVIDE ( _end = . ); |
|||
. = . + _Min_Heap_Size; |
|||
. = . + _Min_Stack_Size; |
|||
. = ALIGN(4); |
|||
} >RAM |
|||
|
|||
/* MEMORY_bank1 section, code must be located here explicitly */ |
|||
/* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ |
|||
.memory_b1_text : |
|||
{ |
|||
*(.mb1text) /* .mb1text sections (code) */ |
|||
*(.mb1text*) /* .mb1text* sections (code) */ |
|||
*(.mb1rodata) /* read-only data (constants) */ |
|||
*(.mb1rodata*) |
|||
} >MEMORY_B1 |
|||
|
|||
/* Remove information from the standard libraries */ |
|||
/DISCARD/ : |
|||
{ |
|||
libc.a ( * ) |
|||
libm.a ( * ) |
|||
libgcc.a ( * ) |
|||
} |
|||
|
|||
.ARM.attributes 0 : { *(.ARM.attributes) } |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue