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

synced serial protocol to multiwii-dev 20120622 

changed booleans to bitfield struct to match with 0622
no other functional changes, and not all enhancements (like boxlight) from 0622 are implemented yet
NOT flight tested, use at your own risk.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@172 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-06-30 13:20:53 +00:00
parent 2fd5645dce
commit 9c2204c179
11 changed files with 5584 additions and 5472 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5117
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

5169
obj/baseflight_fy90q.hex
View File

File diff suppressed because it is too large Load Diff

6
src/buzzer.c
View File

@ -25,12 +25,12 @@ void buzzer(uint8_t warn_vbat)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) {
warn_failsafe = 1; //set failsafe warning level to 1 while landing
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay))
warn_failsafe = 2; //start "find me" signal after landing
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 0)
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED)
warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal
if (failsafeCnt == 0)
warn_failsafe = 0; // turn off alarm if TX is okay
@ -38,7 +38,7 @@ void buzzer(uint8_t warn_vbat)
//===================== GPS fix notification handling =====================
if (sensors(SENSOR_GPS)) {
if ((GPSModeHome || GPSModeHold) && !GPS_fix) { //if no fix and gps funtion is activated: do warning beeps.
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && !f.GPS_FIX) { //if no fix and gps funtion is activated: do warning beeps.
warn_noGPSfix = 1;
} else {
warn_noGPSfix = 0;

2
src/drv_ledring.c
View File

@ -34,7 +34,7 @@ void ledringState(void)
} else if (state == 2) {
b[0] = 'd'; // all unicolor GREEN
b[1] = 1;
if (armed)
if (f.ARMED)
b[2] = 1;
else
b[2] = 0;

14
src/gps.c
View File

@ -212,12 +212,12 @@ void GPS_NewData(uint16_t c)
GPS_update = 0;
else
GPS_update = 1;
if (GPS_fix == 1 && GPS_numSat >= 5) {
if (armed == 0) {
GPS_fix_home = 0;
if (f.GPS_FIX && GPS_numSat >= 5) {
if (!f.ARMED) {
f.GPS_FIX_HOME = 0;
}
if (GPS_fix_home == 0 && armed) {
GPS_fix_home = 1;
if (!f.GPS_FIX_HOME && f.ARMED) {
f.GPS_FIX_HOME = 1;
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
@ -259,7 +259,7 @@ void GPS_NewData(uint16_t c)
//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
if (GPSModeHold == 1 || GPSModeHome == 1) { // ok we are navigating
if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE) { // ok we are navigating
// do gps nav calculations here, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
@ -691,7 +691,7 @@ static bool GPS_newFrame(char c)
} else if (param == 5 && string[0] == 'W')
GPS_coord[LON] = -GPS_coord[LON];
else if (param == 6) {
GPS_fix = string[0] > '0';
f.GPS_FIX = string[0] > '0';
} else if (param == 7) {
GPS_numSat = grab_fields(string, 0);
} else if (param == 9) {

7
src/imu.c
View File

@ -17,7 +17,6 @@ float magneticDeclination = 0.0f; // calculated at startup from config
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int8_t smallAngle25 = 1;
static void getEstimatedAttitude(void);
@ -222,14 +221,14 @@ static void getEstimatedAttitude(void)
rotateV(&EstM.V, deltaGyroAngle);
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
smallAngle25 = 1;
f.SMALL_ANGLES_25 = 1;
else
smallAngle25 = 0;
f.SMALL_ANGLES_25 = 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) {
if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}

4
src/mixer.c
View File

@ -218,7 +218,7 @@ void mixTable(void)
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing
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
@ -289,7 +289,7 @@ void mixTable(void)
else
motor[i] = cfg.mincommand;
}
if (armed == 0)
if (!f.ARMED)
motor[i] = cfg.mincommand;
}
}

130
src/mw.c
View File

@ -1,22 +1,18 @@
#include "board.h"
#include "mw.h"
// March 2012 V2.0
// June 2012 V2-dev
int16_t debug1, debug2, debug3, debug4;
flags_t f;
int16_t debug[4];
uint8_t buzzerState = 0;
uint8_t toggleBeep = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
uint8_t GPSModeHome = 0; // if GPS RTH is activated
uint8_t GPSModeHold = 0; // if GPS PH is activated
uint8_t headFreeMode = 0; // if head free mode is a activated
uint8_t passThruMode = 0; // if passthrough mode is activated
int16_t headFreeModeHold;
int16_t annex650_overrun_count = 0;
uint8_t armed = 0;
uint8_t vbat; // battery voltage in 0.1V steps
int16_t failsafeCnt = 0;
@ -29,10 +25,6 @@ rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) o
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t rcOptions[CHECKBOXITEMS];
uint8_t okToArm = 0;
uint8_t accMode = 0; // if level mode is a activated
uint8_t magMode = 0; // if compass heading hold is a activated
uint8_t baroMode = 0; // if altitude hold is activated
int16_t axisPID[3];
@ -42,7 +34,6 @@ int16_t axisPID[3];
int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint8_t GPS_fix, GPS_fix_home = 0;
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
@ -145,7 +136,7 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (headFreeMode) {
if(f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
@ -176,10 +167,10 @@ void annexCode(void)
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (calibratedACC == 1) {
if (f.ACC_CALIBRATED) {
LED0_OFF;
}
if (armed) {
if (f.ARMED) {
LED0_ON;
}
}
@ -195,12 +186,13 @@ void annexCode(void)
#endif
if (currentTime > calibratedAccTime) {
if (smallAngle25 == 0) {
calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much inclinated
if (!f.SMALL_ANGLES_25) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else
calibratedACC = 1;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
@ -283,16 +275,20 @@ void loop(void)
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) { // Stabilize, and set Throttle to specified level
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
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
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
}
@ -303,13 +299,13 @@ void loop(void)
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && armed == 0) {
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG = 1000;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
}
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (armed == 0 && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
if (rcDelayCommand == 20) {
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
@ -324,30 +320,31 @@ void loop(void)
}
}
} else if (cfg.activate[BOXARM] > 0) {
if (rcOptions[BOXARM] && okToArm) {
armed = 1;
if (rcOptions[BOXARM] && f.OK_TO_ARM) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0
f.ARMED = 1;
headFreeModeHold = heading;
} else if (armed)
armed = 0;
} else if (f.ARMED)
f.ARMED = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && armed == 1) {
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && f.ARMED) {
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] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
armed = 1;
f.ARMED = 1;
headFreeModeHold = heading;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > cfg.maxcheck && armed == 0) {
} else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) {
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20)
calibratingA = 400;
rcDelayCommand++;
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20)
calibratingM = 1; // MAG calibration request
f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) {
cfg.accTrim[PITCH] += 2;
@ -383,7 +380,7 @@ void loop(void)
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
@ -392,7 +389,7 @@ void loop(void)
AccInflightCalibrationArmed = 1;
InflightcalibratingA = 50;
}
} else if (AccInflightCalibrationMeasurementDone && armed == 0) {
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
@ -407,17 +404,17 @@ void loop(void)
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!accMode) {
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
accMode = 1;
f.ACC_MODE = 1;
}
} else
accMode = 0; // failsave support
f.ACC_MODE = 0; // failsave support
if ((rcOptions[BOXARM]) == 0)
okToArm = 1;
if (accMode == 1) {
f.OK_TO_ARM = 1;
if (f.ACC_MODE) {
LED1_ON;
} else {
LED1_OFF;
@ -426,62 +423,63 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else
baroMode = 0;
f.BARO_MODE = 0;
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (magMode == 0) {
magMode = 1;
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else
magMode = 0;
f.MAG_MODE = 0;
if (rcOptions[BOXHEADFREE]) {
if (headFreeMode == 0) {
headFreeMode = 1;
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else
headFreeMode = 0;
f.HEADFREE_MODE = 0;
}
#endif
if (sensors(SENSOR_GPS)) {
if (rcOptions[BOXGPSHOME]) {
if (GPSModeHome == 0) {
GPSModeHome = 1;
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else
GPSModeHome = 0;
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (GPSModeHold == 0) {
GPSModeHold = 1;
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
GPSModeHold = 0;
f.GPS_HOLD_MODE = 0;
}
}
if (rcOptions[BOXPASSTHRU]) {
passThruMode = 1;
f.PASSTHRU_MODE = 1;
} else
passThruMode = 0;
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) {
@ -507,7 +505,7 @@ void loop(void)
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
debug3 = sonarAlt;
debug[2] = sonarAlt;
}
#endif
break;
@ -529,13 +527,13 @@ void loop(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && magMode) {
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
if (smallAngle25)
if (f.SMALL_ANGLES_25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
@ -544,9 +542,9 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (baroMode) {
if (f.BARO_MODE) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
baroMode = 0; // so that a new althold reference is defined
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
@ -555,7 +553,7 @@ void loop(void)
if (sensors(SENSOR_GPS)) {
// Check that we really need to navigate ?
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || (!f.GPS_FIX_HOME)) {
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
@ -567,7 +565,7 @@ void loop(void)
}
// **** PITCH & ROLL & YAW PID ****
for (axis = 0; axis < 3; axis++) {
if (accMode == 1 && axis < 2) { // LEVEL MODE
if (f.ACC_MODE == 1 && 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
#ifdef LEVEL_PDF

53
src/mw.h
View File

@ -94,9 +94,15 @@ 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 CHECKBOXITEMS 11
#define PIDITEMS 10
#define CHECKBOXITEMS 14
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
@ -191,6 +197,24 @@ typedef struct config_t {
uint32_t serial_baudrate;
} config_t;
typedef struct flags_t {
uint8_t OK_TO_ARM :1;
uint8_t ARMED :1;
uint8_t I2C_INIT_DONE :1; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup())
uint8_t ACC_CALIBRATED :1;
uint8_t ACC_MODE :1;
uint8_t MAG_MODE :1;
uint8_t BARO_MODE :1;
uint8_t GPS_HOME_MODE :1;
uint8_t GPS_HOLD_MODE :1;
uint8_t HEADFREE_MODE :1;
uint8_t PASSTHRU_MODE :1;
uint8_t GPS_FIX :1;
uint8_t GPS_FIX_HOME :1;
uint8_t SMALL_ANGLES_25 :1;
uint8_t CALIBRATE_MAG :1;
} flags_t;
extern int16_t gyroZero[3];
extern int16_t gyroData[3];
extern int16_t angle[2];
@ -199,16 +223,13 @@ extern int16_t rcCommand[4];
extern uint8_t rcOptions[CHECKBOXITEMS];
extern int16_t failsafeCnt;
extern int16_t debug1, debug2, debug3, debug4;
extern uint8_t armed;
extern int16_t debug[4];
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
extern uint16_t acc_1G;
extern uint32_t currentTime;
extern uint32_t previousTime;
extern uint16_t cycleTime;
extern uint8_t calibratedACC;
extern uint16_t calibratingA;
extern uint8_t calibratingM;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
@ -219,24 +240,21 @@ extern int32_t EstAlt;
extern int32_t AltHold;
extern int16_t errorAltitudeI;
extern int16_t BaroPID;
extern uint8_t headFreeMode;
extern int16_t headFreeModeHold;
extern uint8_t passThruMode;
extern int8_t smallAngle25;
extern int16_t zVelocity;
extern int16_t heading, magHold;
extern int16_t motor[8];
extern int16_t servo[8];
extern int16_t rcData[8];
extern uint8_t accMode;
extern uint8_t magMode;
extern uint8_t baroMode;
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
extern uint8_t vbat;
extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
extern uint8_t toggleBeep;
// GPS stuff
extern int32_t GPS_coord[2];
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint8_t GPS_fix , GPS_fix_home;
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
@ -246,14 +264,11 @@ extern int16_t GPS_angle[2]; // it's the angles
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 int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern uint8_t vbat;
extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
extern uint8_t toggleBeep;
extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;

6
src/sensors.c
View File

@ -1,10 +1,8 @@
#include "board.h"
#include "mw.h"
uint8_t calibratedACC = 0;
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.
uint16_t calibratingG = 0;
uint8_t calibratingM = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
int16_t heading, magHold;
@ -420,14 +418,14 @@ void Mag_getADC(void)
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
if (calibratingM == 1) {
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
cfg.magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
calibratingM = 0;
f.CALIBRATE_MAG = 0;
}
if (magInit) { // we apply offset only once mag calibration is done

548
src/serial.c
View File

@ -1,6 +1,10 @@
#include "board.h"
#include "mw.h"
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
#define PLATFORM_32BIT 0x80000000
#define MSP_IDENT 100 //out message multitype + version
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation
#define MSP_RAW_IMU 102 //out message 9 DOF
@ -17,6 +21,8 @@
#define MSP_BOX 113 //out message up to 16 checkbox (11 are used)
#define MSP_MISC 114 //out message powermeter trig + 8 free for future use
#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_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
@ -32,7 +38,38 @@
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
static uint8_t checksum, stateMSP, indRX, inBuf[64];
#define INBUF_SIZE 64
static const char boxnames[] =
"ACC;"
"BARO;"
"MAG;"
"CAMSTAB;"
"CAMTRIG;"
"ARM;"
"GPS HOME;"
"GPS HOLD;"
"PASSTHRU;"
"HEADFREE;"
"BEEPER;"
"LEDMAX;"
"LLIGHTS;"
"HEADADJ;";
static const char pidnames[] =
"ROLL;"
"PITCH;"
"YAW;"
"ALT;"
"Pos;"
"PosR;"
"NavR;"
"LEVEL;"
"MAG;"
"VEL;";
static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
static uint8_t cmdMSP;
void serialize32(uint32_t a)
{
@ -68,35 +105,43 @@ void serialize8(uint8_t a)
checksum ^= a;
}
uint32_t read32(void)
{
uint32_t t = inBuf[indRX++];
t += inBuf[indRX++] << 8;
t += (uint32_t) inBuf[indRX++] << 16;
t += (uint32_t) inBuf[indRX++] << 24;
return t;
}
uint16_t read16(void)
{
uint16_t t = inBuf[indRX++];
t += inBuf[indRX++] << 8;
return t;
}
uint8_t read8(void)
{
return inBuf[indRX++] & 0xff;
}
void headSerialReply(uint8_t c, uint8_t s)
uint16_t read16(void)
{
uint16_t t = read8();
t += (uint16_t) read8() << 8;
return t;
}
uint32_t read32(void)
{
uint32_t t = read16();
t += (uint32_t) read16() << 16;
return t;
}
void headSerialResponse(uint8_t err, uint8_t s)
{
serialize8('$');
serialize8('M');
serialize8('>');
serialize8(err ? '!' : '>');
checksum = 0; // start calculating a new checksum
serialize8(s);
serialize8(c);
checksum = 0;
serialize8(cmdMSP);
}
void headSerialReply(uint8_t s)
{
headSerialResponse(0, s);
}
void headSerialError(uint8_t s)
{
headSerialResponse(1, s);
}
void tailSerialReply(void)
@ -106,6 +151,13 @@ void tailSerialReply(void)
// UartSendData();
}
void serializeNames(const char *s)
{
const char *c;
for (c = s; *c; c++)
serialize8(*c);
}
// signal that we're in cli mode
uint8_t cliMode = 0;
@ -114,10 +166,203 @@ void serialInit(uint32_t baudrate)
uartInit(baudrate);
}
static void evaluateCommand(void)
{
uint8_t i;
switch (cmdMSP) {
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++)
rcData[i] = read16();
headSerialReply(0);
break;
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
case MSP_SET_PID:
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = read16();
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
cfg.rcRate8 = read8();
cfg.rcExpo8 = read8();
cfg.rollPitchRate = read8();
cfg.yawRate = read8();
cfg.dynThrPID = read8();
cfg.thrMid8 = read8();
cfg.thrExpo8 = read8();
headSerialReply(0);
break;
case MSP_SET_MISC:
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(cfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(10);
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);
break;
case MSP_RAW_IMU:
headSerialReply(18);
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]);
break;
case MSP_SERVO:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(servo[i]);
break;
case MSP_MOTOR:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
break;
case MSP_RC:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(rcData[i]);
break;
case MSP_RAW_GPS:
headSerialReply(14);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
break;
case MSP_ATTITUDE:
headSerialReply(8);
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize16(heading);
serialize16(headFreeModeHold);
break;
case MSP_ALTITUDE:
headSerialReply(4);
serialize32(EstAlt);
break;
case MSP_BAT:
headSerialReply(3);
serialize8(vbat);
serialize16(0); // power meter trash
break;
case MSP_RC_TUNING:
headSerialReply(7);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
serialize8(cfg.thrMid8);
serialize8(cfg.thrExpo8);
break;
case MSP_PID:
headSerialReply(3 * PIDITEMS);
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
break;
case MSP_BOX:
headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
serialize16(cfg.activate[i]);
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_MISC:
headSerialReply(2);
serialize16(0); // intPowerTrigger1
break;
case MSP_MOTOR_PINS:
headSerialReply(8);
for (i = 0; i < 8; i++)
serialize8(i + 1);
break;
case MSP_RESET_CONF:
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeParams(0);
headSerialReply(0);
break;
case MSP_DEBUG:
headSerialReply(8);
for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
void serialCom(void)
{
uint8_t i, c;
static uint8_t offset, dataSize;
uint8_t c;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode) {
@ -128,238 +373,41 @@ void serialCom(void)
while (uartAvailable()) {
c = uartRead();
if (stateMSP > 99) { // a message with a length indication, indicating a non null payload
if (offset <= dataSize) { // there are still some octets to read (including checksum) to complete a full message
if (offset < dataSize)
checksum ^= c; // the checksum is computed, except for the last octet
inBuf[offset++] = c;
} else { // we have read all the payload
if (checksum == inBuf[dataSize]) { // we check is the computed checksum is ok
switch (stateMSP) { // if yes, then we execute different code depending on the message code. read8/16/32 will look into the inBuf buffer
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++) {
rcData[i] = read16();
}
break;
case MSP_SET_RAW_GPS:
GPS_fix = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update = 1;
break;
case MSP_SET_PID:
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++) {
cfg.activate[i] = read16();
}
break;
case MSP_SET_RC_TUNING:
cfg.rcRate8 = read8();
cfg.rcExpo8 = read8();
cfg.rollPitchRate = read8();
cfg.yawRate = read8();
cfg.dynThrPID = read8();
cfg.thrMid8 = read8();
cfg.thrExpo8 = read8();
break;
case MSP_SET_MISC:
break;
}
}
stateMSP = 0; // in any case we reset the MSP state
}
}
if (stateMSP < 5) {
if (stateMSP == 4) { // this protocol state indicates we have a message with a lenght indication, and we read here the message code (fifth octet)
if (c > 99) { // we check if it's a valid code (should be >99)
stateMSP = c; // the message code is then reuse to feed the protocol state
offset = 0;
checksum = 0;
indRX = 0; // and we init some values which will be used in the next loops to grasp the payload
} else {
stateMSP = 0; // the message code seems to be invalid. this should not happen => we reset the protocol state
}
}
if (stateMSP == 3) { // here, we need to check if the fourth octet indicates a code indication (>99) or a payload lenght indication (<100)
if (c < 100) { // a message with a length indication, indicating a non null payload
stateMSP++; // we update the protocol state to read the next octet
dataSize = c; // we store the payload lenght
if (dataSize > 63)
dataSize = 63; // in order to avoid overflow, we limit the size. this should not happen
} else {
switch (c) { // if we are here, the fourth octet indicates a code message
case MSP_IDENT: // and we check message code to execute the relative code
headSerialReply(c, 2); // we reply with an header indicating a payload lenght of 2 octets
serialize8(VERSION); // the first octet. serialize8/16/32 is used also to compute a checksum
serialize8(cfg.mixerConfiguration); // the second one
tailSerialReply();
break; // mainly to send the last octet which is the checksum
case MSP_STATUS:
headSerialReply(c, 8);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize16(accMode << BOXACC | baroMode << BOXBARO | magMode << BOXMAG | armed << BOXARM | GPSModeHome << BOXGPSHOME | GPSModeHold << BOXGPSHOLD | headFreeMode << BOXHEADFREE);
tailSerialReply();
break;
case MSP_RAW_IMU:
headSerialReply(c, 18);
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]);
tailSerialReply();
break;
case MSP_SERVO:
headSerialReply(c, 16);
for (i = 0; i < 8; i++)
serialize16(servo[i]);
tailSerialReply();
break;
case MSP_MOTOR:
headSerialReply(c, 16);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
tailSerialReply();
break;
case MSP_RC:
headSerialReply(c, 16);
for (i = 0; i < 8; i++)
serialize16(rcData[i]);
tailSerialReply();
break;
case MSP_RAW_GPS:
headSerialReply(c, 14);
serialize8(GPS_fix);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
tailSerialReply();
break;
case MSP_COMP_GPS:
headSerialReply(c, 5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome + 180);
serialize8(GPS_update);
tailSerialReply();
break;
case MSP_ATTITUDE:
headSerialReply(c, 6);
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize16(heading);
tailSerialReply();
break;
case MSP_ALTITUDE:
headSerialReply(c, 4);
serialize32(EstAlt);
tailSerialReply();
break;
case MSP_BAT:
headSerialReply(c, 3);
serialize8(vbat);
serialize16(0);
tailSerialReply();
break;
case MSP_RC_TUNING:
headSerialReply(c, 7);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
serialize8(cfg.thrMid8);
serialize8(cfg.thrExpo8);
tailSerialReply();
break;
case MSP_PID:
headSerialReply(c, 3 * PIDITEMS);
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
tailSerialReply();
break;
case MSP_BOX:
headSerialReply(c, 2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++) {
serialize16(cfg.activate[i]);
}
tailSerialReply();
break;
case MSP_MISC:
headSerialReply(c, 2);
serialize16(0);
tailSerialReply();
break;
case MSP_MOTOR_PINS:
headSerialReply(c, 8);
for (i = 0; i < 8; i++)
serialize8(i + 1);
tailSerialReply();
break;
case MSP_RESET_CONF:
checkFirstTime(true);
break;
case MSP_ACC_CALIBRATION:
calibratingA = 400;
calibratingG = 1000; // hit gyro too
break;
case MSP_MAG_CALIBRATION:
calibratingM = 1;
break;
case MSP_EEPROM_WRITE:
writeParams(0);
break;
case MSP_DEBUG:
headSerialReply(c, 8);
serialize16(debug1); // 4 variables are here for general monitoring purpose
serialize16(debug2);
serialize16(debug3);
serialize16(debug4);
tailSerialReply();
break;
}
stateMSP = 0; // we reset the protocol state for the next loop
}
} else {
switch (c) { // header detection $MW<
case '$':
if (stateMSP == 0)
stateMSP++;
break; // first octet ok, no need to go further
case 'M':
if (stateMSP == 1)
stateMSP++;
break; // second octet ok, no need to go further
case '<':
if (stateMSP == 2)
stateMSP++;
break; // third octet ok, no need to go further
}
}
}
if (stateMSP == 0) { // still compliant with older single octet command
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;
} else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {
c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state = IDLE;
continue;
}
dataSize = c;
offset = 0;
checksum = 0;
indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow
} else if (c_state == HEADER_SIZE) {
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
} else if (c_state == HEADER_CMD && offset < dataSize) {
checksum ^= c;
inBuf[offset++] = c;
} else if (c_state == HEADER_CMD && offset >= dataSize) {
if (checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state = IDLE;
}
}
}