Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

synced with mwc 2.1. it's suprising how many "new" things in 2.1 didn't actually matter on a real platform.

Browse source 
removed camtrig stuff since it wasnt possible. somewhat replaced with aux forwarding (see below)
2.1 buzzer code changed, untested.
removed flying wing mixer. nobody used that. 
added alt_hold_throttle_neutral, nav_slew_rate and looptime configuration to cli. default looptime set to 3000. changed default gyro_cmpf to 400 to sync with 2.1.
increased bmp085 oversampling
added gimbal_flags (bit 4 set) flag which, in PPM mode, forwards AUX1..4 to the lower 4 PWM outputs instead of using them as motors. set gimbal_flags=8 to test it out. output is fixed to 50Hz.
merged 2.1 gps changes (not many)
casting in gyro smoothing (nobody uses that anyway)
calibrate accel in gimbal mode, set smallangle in gyro-only mode
vtail4 mixer fix
flight tested on quadx w/ppm.


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@182 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-07-20 14:53:15 +00:00
parent e70d7b5d16
commit c98113b82c
19 changed files with 5363 additions and 5696 deletions
Download patch file Download diff file Expand all files Collapse all files

4952
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

5307
obj/baseflight_fy90q.hex
View file

File diff suppressed because it is too large Load diff

15
src/board.h
View file

@ -15,7 +15,7 @@
#include "core_cm3.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#define M_PI 3.14159265358979323846f
#endif /* M_PI */
typedef enum {
@ -41,13 +41,12 @@ typedef enum {
FEATURE_SPEKTRUM = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_CAMTRIG = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7,
FEATURE_LED_RING = 1 << 8,
FEATURE_GPS = 1 << 9,
FEATURE_FAILSAFE = 1 << 10,
FEATURE_SONAR = 1 << 11,
FEATURE_TELEMETRY = 1 << 12,
FEATURE_GYRO_SMOOTHING = 1 << 6,
FEATURE_LED_RING = 1 << 7,
FEATURE_GPS = 1 << 8,
FEATURE_FAILSAFE = 1 << 9,
FEATURE_SONAR = 1 << 10,
FEATURE_TELEMETRY = 1 << 11,
} AvailableFeatures;
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype

160
src/buzzer.c
View file

@ -1,22 +1,18 @@
#include "board.h"
#include "mw.h"
static uint8_t buzzerIsOn = 0, beepDone = 0;
static uint32_t buzzerLastToggleTime;
static void beep(uint16_t pulse);
static void beep_code(char first, char second, char third, char pause);
void buzzer(uint8_t warn_vbat)
{
static uint16_t ontime, offtime, beepcount, repeat, repeatcounter;
static uint32_t buzzerLastToggleTime;
static uint8_t buzzerIsOn = 0, activateBuzzer, beeperOnBox, i, last_rcOptions[CHECKBOXITEMS], warn_noGPSfix = 0, warn_failsafe = 0;
static uint8_t beeperOnBox;
static uint8_t warn_noGPSfix = 0;
static uint8_t warn_failsafe = 0;
static uint8_t warn_runtime = 0;
//===================== Beeps for changing rcOptions =====================
#if defined(RCOPTIONSBEEP)
if (last_rcOptions[i] != rcOptions[i]) {
toggleBeep = 1;
}
last_rcOptions[i] = rcOptions[i];
i++;
if (i >= CHECKBOXITEMS)
i = 0;
#endif
//===================== BeeperOn via rcOptions =====================
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
beeperOnBox = 1;
@ -38,79 +34,95 @@ void buzzer(uint8_t warn_vbat)
//===================== GPS fix notification handling =====================
if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && !f.GPS_FIX) { //if no fix and gps funtion is activated: do warning beeps.
if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !f.GPS_FIX) { // if no fix and gps funtion is activated: do warning beeps
warn_noGPSfix = 1;
} else {
warn_noGPSfix = 0;
}
}
//===================== Main Handling Block =====================
repeat = 1; // set repeat to default
ontime = 100; // set offtime to default
//the order of the below is the priority from high to low, the last entry has the lowest priority, only one option can be active at the same time
if (warn_failsafe == 2) {
activateBuzzer = 1;
offtime = 2000;
ontime = 300;
repeat = 1;
} //failsafe "find me" signal
else if (warn_failsafe == 1) {
activateBuzzer = 1;
offtime = 50;
} //failsafe landing active
else if (warn_noGPSfix == 1) {
activateBuzzer = 1;
offtime = 10;
} else if (beeperOnBox == 1) {
activateBuzzer = 1;
offtime = 50;
} //beeperon
else if (warn_vbat == 4) {
activateBuzzer = 1;
offtime = 500;
repeat = 3;
} else if (warn_vbat == 2) {
activateBuzzer = 1;
offtime = 1000;
repeat = 2;
} else if (warn_vbat == 1) {
activateBuzzer = 1;
offtime = 2000;
} else if (toggleBeep > 0) {
activateBuzzer = 1;
ontime = 50;
offtime = 50;
} //fast confirmation beep
//===================== Priority driven Handling =====================
// beepcode(length1,length2,length3,pause)
// D: Double, L: Long, M: Middle, S: Short, N: None
if (warn_failsafe == 2)
beep_code('L','N','N','D'); // failsafe "find me" signal
else if (warn_failsafe == 1)
beep_code('S','M','L','M'); // failsafe landing active
else if (warn_noGPSfix == 1)
beep_code('S','S','N','M');
else if (beeperOnBox == 1)
beep_code('S','S','S','M'); // beeperon
else if (warn_vbat == 4)
beep_code('S','M','M','D');
else if (warn_vbat == 2)
beep_code('S','S','M','D');
else if (warn_vbat == 1)
beep_code('S','M','N','D');
else if (warn_runtime == 1 && f.ARMED)
beep_code('S','S','M','N'); // Runtime warning
else if (toggleBeep > 0)
beep(50); // fast confirmation beep
else {
activateBuzzer = 0;
buzzerIsOn = 0;
BEEP_OFF;
}
}
if (activateBuzzer) {
if (repeatcounter > 1 && !buzzerIsOn && (millis() >= (buzzerLastToggleTime + 80))) { // if the buzzer is off and there is a short pause neccessary (multipe buzzes)
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
repeatcounter--;
} else if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + offtime))) { // Buzzer is off and long pause time is up -> turn it on
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
repeatcounter = repeat; //set the amount of repeats
} else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + ontime)) { //Buzzer is on and time is up -> turn it off
void beep_code(char first, char second, char third, char pause)
{
char patternChar[4];
uint16_t Duration;
static uint8_t icnt = 0;
patternChar[0] = first;
patternChar[1] = second;
patternChar[2] = third;
patternChar[3] = pause;
switch(patternChar[icnt]) {
case 'M':
Duration = 100;
break;
case 'L':
Duration = 200;
break;
case 'D':
Duration = 2000;
break;
case 'N':
Duration = 0;
break;
default:
Duration = 50;
break;
}
if (icnt < 3 && Duration != 0)
beep(Duration);
if (icnt >= 3 && (buzzerLastToggleTime < millis() - Duration)) {
icnt = 0;
toggleBeep = 0;
}
if (beepDone == 1 || Duration == 0) {
if (icnt < 3)
icnt++;
beepDone = 0;
buzzerIsOn = 0;
BEEP_OFF;
}
}
static void beep(uint16_t pulse)
{
if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + 50))) { // Buzzer is off and long pause time is up -> turn it on
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
} else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + pulse)) { // Buzzer is on and time is up -> turn it off
buzzerIsOn = 0;
BEEP_OFF;
buzzerLastToggleTime = millis();
if (toggleBeep >0)
beepcount++; // only increment if confirmation beep, the rest is endless while the condition is given
}
if (beepcount >= toggleBeep) { //confirmation flag is 0,1 or 2
beepcount = 0; //reset the counter for the next time
toggleBeep = 0; //reset the flag after all beeping is done
}
} else { //no beeping neccessary:reset everything (just in case)
beepcount = 0; //reset the counter for the next time
BEEP_OFF;
buzzerIsOn = 0;
toggleBeep--;
beepDone = 1;
}
}

7
src/cli.c
View file

@ -36,7 +36,7 @@ const char *mixerNames[] = {
// sync this with AvailableFeatures enum from board.h
const char *featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY",
NULL
};
@ -91,6 +91,7 @@ typedef struct {
const clivalue_t valueTable[] = {
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
{ "midrc", VAR_UINT16, &cfg.midrc, 1200, 1700 },
{ "minthrottle", VAR_UINT16, &cfg.minthrottle, 0, 2000 },
{ "maxthrottle", VAR_UINT16, &cfg.maxthrottle, 0, 2000 },
@ -110,8 +111,6 @@ const clivalue_t valueTable[] = {
{ "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "wing_left_mid", VAR_UINT16, &cfg.wing_left_mid, 0, 2000 },
{ "wing_right_mid", VAR_UINT16, &cfg.wing_right_mid, 0, 2000 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
@ -142,6 +141,8 @@ const clivalue_t valueTable[] = {
{ "nav_controls_heading", VAR_UINT8, &cfg.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 },
{ "looptime", VAR_UINT16, &cfg.looptime, 1500, 9000 },
{ "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 },

23
src/config.c
View file

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 22;
uint8_t checkNewConf = 24;
void parseRcChannels(const char *input)
{
@ -47,8 +47,6 @@ void readEEPROM(void)
lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}
cfg.wing_left_mid = constrain(cfg.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
cfg.wing_right_mid = constrain(cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
}
@ -89,8 +87,9 @@ void checkFirstTime(bool reset)
cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT); // | FEATURE_PPM); // sadly, this is for hackers only
featureSet(FEATURE_VBAT);
cfg.looptime = 3000;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
@ -112,13 +111,13 @@ void checkFirstTime(bool reset)
cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.P8[PIDLEVEL] = 70;
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 90;
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;
@ -128,15 +127,15 @@ void checkFirstTime(bool reset)
cfg.thrExpo8 = 0;
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = 0;
cfg.accTrim[0] = 0;
cfg.accTrim[1] = 0;
cfg.angleTrim[0] = 0;
cfg.angleTrim[1] = 0;
cfg.accZero[0] = 0;
cfg.accZero[1] = 0;
cfg.accZero[2] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.gyro_cmpf_factor = 310; // default MWC
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
@ -147,6 +146,7 @@ void checkFirstTime(bool reset)
parseRcChannels("AETR1234");
cfg.deadband = 0;
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
@ -167,8 +167,6 @@ void checkFirstTime(bool reset)
// servos
cfg.yaw_direction = 1;
cfg.wing_left_mid = 1500;
cfg.wing_right_mid = 1500;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
@ -188,6 +186,7 @@ void checkFirstTime(bool reset)
cfg.gps_baudrate = 9600;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;

2
src/drv_bmp085.c
View file

@ -125,7 +125,7 @@ bool bmp085Init(void)
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->oversampling_setting = 2;
p_bmp085->oversampling_setting = 3;
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;

3
src/drv_i2c_soft.c
View file

@ -19,10 +19,9 @@
static void I2C_delay(void)
{
volatile int i = 7;
while (i) {
while (i)
i--;
}
}
static bool I2C_Start(void)
{

5
src/drv_pwm.c
View file

@ -366,7 +366,8 @@ bool pwmInit(drv_pwm_config_t *init)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1;
// when in extra servos mode, init lower 4 channels as 50Hz outputs
TIM_TimeBaseStructure.TIM_Period = (1000000 / (init->extraServos ? 50 : init->motorPwmRate)) - 1;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
@ -388,7 +389,7 @@ bool pwmInit(drv_pwm_config_t *init)
TIM_Cmd(TIM3, ENABLE);
TIM_CtrlPWMOutputs(TIM3, ENABLE);
// configure number of PWM outputs, in PPM/spektrum mode, we use bottom 4 channels more more motors
numOutputChannels = 10;
numOutputChannels = init->extraServos ? 6 : 10;
}
#if 0

1
src/drv_pwm.h
View file

@ -4,6 +4,7 @@ typedef struct drv_pwm_config_t {
bool enableInput;
bool usePPM;
bool useServos;
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
uint16_t motorPwmRate;
uint16_t servoPwmRate;
} drv_pwm_config_t;

71
src/gps.c
View file

@ -12,6 +12,10 @@ void gpsInit(uint32_t baudrate)
{
GPS_set_pids();
uart2Init(baudrate, GPS_NewData);
// catch some GPS frames. TODO check this
delay(500);
if (GPS_Present)
sensorsSet(SENSOR_GPS);
}
@ -42,7 +46,7 @@ static void GPS_calc_nav_rate(int max_speed);
static void GPS_update_crosstrack(void);
static bool GPS_newFrame(char c);
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
static int32_t wrap_18000(int32_t error);
int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
@ -140,7 +144,6 @@ static AC_PID pid_nav[2];
#define RADX100 0.000174532925f
#define CROSSTRACK_GAIN 1
#define NAV_SPEED_MAX 300 // cm/sec
#define NAV_SLOW_NAV true
#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
@ -201,7 +204,7 @@ static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c)
{
uint8_t axis;
int axis;
static uint32_t nav_loopTimer;
uint32_t dist;
int32_t dir;
@ -225,7 +228,7 @@ void GPS_NewData(uint16_t c)
}
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = ++GPS_filter_index % GPS_FILTER_VECTOR_LENGTH;
GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
@ -300,14 +303,19 @@ void GPS_NewData(uint16_t c)
void GPS_reset_home_position(void)
{
if (f.GPS_FIX && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
nav_takeoff_bearing = heading; //save takeoff heading
//Set ground altitude
f.GPS_FIX_HOME = 1;
}
}
//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void)
{
uint8_t i;
int i;
for (i = 0; i < 2; i++) {
GPS_angle[i] = 0;
@ -454,12 +462,12 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold()
static void GPS_calc_poshold(void)
{
int32_t p, i, d;
int32_t output;
int32_t target_speed;
uint8_t axis;
int axis;
for (axis = 0; axis < 2; axis++) {
target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID); // calculate desired speed from lon error
@ -488,7 +496,7 @@ static void GPS_calc_nav_rate(int max_speed)
{
float trig[2];
float temp;
uint8_t axis;
int axis;
// push us towards the original track
GPS_update_crosstrack();
@ -518,7 +526,7 @@ static void GPS_update_crosstrack(void)
{
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
@ -560,7 +568,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
////////////////////////////////////////////////////////////////////////////////////
// Utilities
//
static int32_t wrap_18000(int32_t error)
int32_t wrap_18000(int32_t error)
{
if (error > 18000)
error -= 36000;
@ -592,7 +600,6 @@ static int32_t wrap_36000(int32_t angle)
EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
resolution also increased precision of nav calculations
*/
static uint32_t GPS_coord_to_degrees(char *s)
{
char *p = s, *d = s;
@ -617,6 +624,46 @@ static uint32_t GPS_coord_to_degrees(char *s)
min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
}
*/
#define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* s)
{
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
int i;
// scan for decimal point or end of field
for (p = s; isdigit(*p); p++)
;
q = s;
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 10000000UL + (min * 1000000UL + frac_min * 100UL) / 6;
}
// helper functions
static uint32_t grab_fields(char *src, uint8_t mult)
@ -699,7 +746,7 @@ static bool GPS_newFrame(char c)
}
} else if (frame == FRAME_RMC) {
if (param == 7) {
GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L; // speed in cm/s added by Mis
GPS_speed = ((uint32_t) grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
}
}
param++;

12
src/imu.c
View file

@ -72,7 +72,7 @@ void computeIMU(void)
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;
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
@ -89,11 +89,11 @@ void computeIMU(void)
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis];
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
}
}
@ -127,12 +127,6 @@ void computeIMU(void)
/* 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
// #define GYR_CMPF_FACTOR 500.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*/

4
src/main.c
View file

@ -79,6 +79,7 @@ int main(void)
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
@ -127,7 +128,10 @@ int main(void)
}
previousTime = micros();
if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
f.SMALL_ANGLES_25 = 1;
// loopy
while (1) {

47
src/mixer.c
View file

@ -12,7 +12,7 @@ void mixerInit(void)
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
useServo = 1;
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CAMTRIG))
if (feature(FEATURE_SERVO_TILT))
useServo = 1;
switch (cfg.mixerConfiguration) {
@ -98,9 +98,6 @@ void mixTable(void)
{
int16_t maxMotor;
uint32_t i;
static uint8_t camCycle = 0;
static uint8_t camState = 0;
static uint32_t camTime = 0;
if (numberMotor > 3) {
// prevent "yaw jump" during yaw correction
@ -214,9 +211,9 @@ void mixTable(void)
break;
case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
motor[0] = PIDMIX(+0, +1, +1); //REAR_R
motor[1] = PIDMIX(-1, -1, +0); //FRONT_R
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
motor[2] = PIDMIX(+0, +1, -1); //REAR_L
motor[3] = PIDMIX(+1, -1, -0); //FRONT_L
break;
@ -224,19 +221,6 @@ void mixTable(void)
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
break;
}
// do camstab
@ -260,28 +244,9 @@ void mixTable(void)
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
}
// do camtrig (this doesn't actually work)
if (feature(FEATURE_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 (rcOptions[BOXCAMTRIG])
camCycle = 1;
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
for (i = 0; i < 4; i++)
pwmWrite(6 + i, rcData[AUX1 + i]);
}
maxMotor = motor[0];

86
src/mw.c
View file

@ -1,11 +1,10 @@
#include "board.h"
#include "mw.h"
// June 2012 V2-dev
// July 2012 V2.1
flags_t f;
int16_t debug[4];
uint8_t buzzerState = 0;
uint8_t toggleBeep = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
@ -36,14 +35,16 @@ int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees*10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
//Automatic ACC Offset Calibration
@ -80,8 +81,8 @@ void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
@ -152,12 +153,8 @@ void annexCode(void)
vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8);
}
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
buzzerFreq = 7;
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
buzzerState = 0;
BEEP_OFF;
} else
buzzerFreq = 4; // low battery
}
@ -167,9 +164,8 @@ void annexCode(void)
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (f.ACC_CALIBRATED) {
if (f.ACC_CALIBRATED)
LED0_OFF;
}
if (f.ARMED)
LED0_ON;
// This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
@ -267,10 +263,8 @@ void loop(void)
static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
#ifdef TIMINGDEBUG
trollTime = micros();
#endif
static uint32_t loopTime;
uint16_t auxState = 0;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
@ -356,28 +350,28 @@ void loop(void)
f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) {
cfg.accTrim[PITCH] += 2;
cfg.angleTrim[PITCH] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[PITCH] < cfg.mincheck) {
cfg.accTrim[PITCH] -= 2;
cfg.angleTrim[PITCH] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] > cfg.maxcheck) {
cfg.accTrim[ROLL] += 2;
cfg.angleTrim[ROLL] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] < cfg.mincheck) {
cfg.accTrim[ROLL] -= 2;
cfg.angleTrim[ROLL] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
@ -394,21 +388,18 @@ void loop(void)
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationArmed) {
AccInflightCalibrationArmed = 1;
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
}
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
}
for (i = 0; i < CHECKBOXITEMS; i++) {
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2
| (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5 | (rcData[AUX3] < 1300) << 6 | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 7 | (rcData[AUX3] > 1700) << 8 | (rcData[AUX4] < 1300) << 9 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 10 | (rcData[AUX4] > 1700) << 11) & cfg.activate[i]) > 0;
}
for(i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for(i = 0; i < CHECKBOXITEMS; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
@ -457,12 +448,17 @@ void loop(void)
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
}
#endif
if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
@ -484,11 +480,13 @@ void loop(void)
f.GPS_HOLD_MODE = 0;
}
}
}
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
} else
} else {
f.PASSTHRU_MODE = 0;
}
} else { // not in rc loop
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
switch (taskOrder++ % 4) {
@ -524,6 +522,10 @@ void loop(void)
}
}
currentTime = micros();
if (currentTime > loopTime) {
loopTime = currentTime + cfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
@ -568,15 +570,22 @@ void loop(void)
} else {
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]),-cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
}
// **** PITCH & ROLL & YAW PID ****
for (axis = 0; axis < 3; axis++) {
if (f.ACC_MODE == 1 && axis < 2) { // LEVEL MODE
if (f.ACC_MODE && axis < 2) { // LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
#ifdef LEVEL_PDF
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
@ -613,18 +622,5 @@ void loop(void)
mixTable();
writeServos();
writeMotors();
#ifdef TIMINGDEBUG
while (micros() < trollTime + 1750);
// LED0_TOGGLE;
{
if (cycleTime < cn)
cn = cycleTime;
if (cycleTime > cx)
cx = cycleTime;
debug1 = cn;
debug2 = cx;
}
#endif
}

50
src/mw.h
View file

@ -1,28 +1,9 @@
#pragma once
/* 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_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]
/* experimental
camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */
#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
/* for VBAT monitoring frequency */
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define VERSION 200
#define VERSION 210
#define LAT 0
#define LON 1
@ -31,7 +12,7 @@
// navigation mode
typedef enum NavigationMode
{
NAV_MODE_NONE = 1,
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} NavigationMode;
@ -46,7 +27,7 @@ typedef enum MultiType
MULTITYPE_GIMBAL = 5,
MULTITYPE_Y6 = 6,
MULTITYPE_HEX6 = 7,
MULTITYPE_FLYING_WING = 8,
MULTITYPE_FLYING_WING = 8, // UNSUPPORTED, do not select!
MULTITYPE_Y4 = 9,
MULTITYPE_HEX6X = 10,
MULTITYPE_OCTOX8 = 11,
@ -63,6 +44,7 @@ typedef enum GimbalFlags {
GIMBAL_NORMAL = 1 << 0,
GIMBAL_TILTONLY = 1 << 1,
GIMBAL_DISABLEAUX34 = 1 << 2,
GIMBAL_FORWARDAUX = 1 << 3,
} GimbalFlags;
/*********** RC alias *****************/
@ -94,12 +76,9 @@ typedef enum GimbalFlags {
#define BOXPASSTHRU 8
#define BOXHEADFREE 9
#define BOXBEEPERON 10
/* we want maximum illumination */
#define BOXLEDMAX 11
/* enable landing lights at any altitude */
#define BOXLLIGHTS 12
/* acquire heading for HEADFREE mode */
#define BOXHEADADJ 13
#define BOXLEDMAX 11 // we want maximum illumination
#define BOXLLIGHTS 12 // enable landing lights at any altitude
#define BOXHEADADJ 13 // acquire heading for HEADFREE mode
#define PIDITEMS 10
#define CHECKBOXITEMS 14
@ -114,6 +93,8 @@ typedef struct config_t {
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint16_t looptime; // imu loop time in us
uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS];
@ -130,7 +111,7 @@ typedef struct config_t {
int16_t accZero[3];
int16_t magZero[3];
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t accTrim[2];
int16_t angleTrim[2]; // accelerometer trim
// sensor-related stuff
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
@ -148,6 +129,7 @@ typedef struct config_t {
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-20
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
@ -168,8 +150,6 @@ typedef struct config_t {
// mixer-related configuration
int8_t yaw_direction;
uint16_t wing_left_mid; // left servo center pos. - use this for initial trim
uint16_t wing_right_mid; // right servo center pos. - use this for initial trim
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
uint16_t tri_yaw_min; // tail servo min
uint16_t tri_yaw_max; // tail servo max
@ -189,6 +169,7 @@ typedef struct config_t {
uint32_t gps_baudrate;
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec
@ -257,15 +238,17 @@ extern int32_t GPS_coord[2];
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome,GPS_distanceToHold; // distance to home or hold point in meters
extern int16_t GPS_directionToHome,GPS_directionToHold; // direction to home or hol point in degrees
extern uint16_t GPS_distanceToHome; // distance to home or hold point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m and speed in 0.1m/s
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern config_t cfg;
extern flags_t f;
@ -335,6 +318,7 @@ void gpsInit(uint32_t baudrate);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error);
// telemetry
void initTelemetry(bool State);

25
src/sensors.c
View file

@ -138,7 +138,7 @@ void batteryInit(void)
static void ACC_Common(void)
{
static int32_t a[3];
uint32_t axis;
int axis;
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
@ -156,8 +156,8 @@ static void ACC_Common(void)
cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0;
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
}
calibratingA--;
@ -166,17 +166,16 @@ static void ACC_Common(void)
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t accTrim_saved[2] = { 0, 0 };
static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = cfg.accZero[ROLL];
accZero_saved[PITCH] = cfg.accZero[PITCH];
accZero_saved[YAW] = cfg.accZero[YAW];
accTrim_saved[ROLL] = cfg.accTrim[ROLL];
accTrim_saved[PITCH] = cfg.accTrim[PITCH];
angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
}
if (InflightcalibratingA > 0) {
uint8_t axis;
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
@ -196,8 +195,8 @@ static void ACC_Common(void)
cfg.accZero[ROLL] = accZero_saved[ROLL];
cfg.accZero[PITCH] = accZero_saved[PITCH];
cfg.accZero[YAW] = accZero_saved[YAW];
cfg.accTrim[ROLL] = accTrim_saved[ROLL];
cfg.accTrim[PITCH] = accTrim_saved[PITCH];
cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
}
InflightcalibratingA--;
}
@ -207,8 +206,8 @@ static void ACC_Common(void)
cfg.accZero[ROLL] = b[ROLL] / 50;
cfg.accZero[PITCH] = b[PITCH] / 50;
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0;
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
}
}
@ -254,7 +253,7 @@ void Baro_update(void)
case 2:
bmp085_start_up();
baroState++;
baroDeadline += 14000;
baroDeadline += 26000;
break;
case 3:
baroUP = bmp085_get_up();
@ -272,7 +271,7 @@ static void GYRO_Common(void)
{
static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
uint32_t axis;
int axis;
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {

51
src/serial.c
View file

@ -23,6 +23,7 @@
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
#define MSP_BOXNAMES 116 //out message the aux switch names
#define MSP_PIDNAMES 117 //out message the PID names
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
@ -33,6 +34,7 @@
#define MSP_MAG_CALIBRATION 206 //in message no param
#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param
#define MSP_WP_SET 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_EEPROM_WRITE 250 //in message no param
@ -71,6 +73,8 @@ static const char pidnames[] =
static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
static uint8_t cmdMSP;
static bool guiConnected = false;
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialize32(uint32_t a)
{
@ -148,8 +152,6 @@ void headSerialError(uint8_t s)
void tailSerialReply(void)
{
serialize8(checksum);
// no need to send
// UartSendData();
}
void serializeNames(const char *s)
@ -159,9 +161,6 @@ void serializeNames(const char *s)
serialize8(*c);
}
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialInit(uint32_t baudrate)
{
uartInit(baudrate);
@ -170,6 +169,7 @@ void serialInit(uint32_t baudrate)
static void evaluateCommand(void)
{
uint32_t i;
uint8_t wp_no;
switch (cmdMSP) {
case MSP_SET_RAW_RC:
@ -225,7 +225,9 @@ static void evaluateCommand(void)
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU | rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
break;
case MSP_RAW_IMU:
headSerialReply(18);
@ -322,6 +324,23 @@ static void evaluateCommand(void)
for (i = 0; i < 8; i++)
serialize8(i + 1);
break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
if (wp_no == 0) {
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
} else if (wp_no == 16) {
serialize8(16); // wp16
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
break;
case MSP_RESET_CONF:
checkFirstTime(true);
headSerialReply(0);
@ -350,6 +369,18 @@ static void evaluateCommand(void)
tailSerialReply();
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
switch (sr) {
case '#':
cliProcess();
break;
case 'R':
systemReset(true); // reboot to bootloader
break;
}
}
void serialCom(void)
{
@ -375,13 +406,9 @@ void serialCom(void)
c = uartRead();
if (c_state == IDLE) {
// still compliant with older single octet command
// enable CLI
if (c == '#')
cliProcess();
else if (c == 'R')
systemReset(true); // reboot to bootloader
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE)
evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {