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

trashed uvop keil file since that was system-specific stuff. 

applied mwc-dev GPS code with bits of tarducopter code by sbaron. Thanks again. Moved some of the GPS config stuff into cli - gps_lpf, min/max nav speed, nav_controls_heading. Remember I don't test any GPS functionality at all, so if this makes your quad fly towards North Korea at over 9000cm/sec, this is NOT my problem.
spacing fixes in a couple files.
trashed old serial code that was under #if 0


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@161 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-06-06 15:09:44 +00:00
parent 12d6c41407
commit 19ca85963b
9 changed files with 6546 additions and 6853 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1329
baseflight.uvopt
View File

File diff suppressed because it is too large Load Diff

5458
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

5460
obj/baseflight_fy90q.hex
View File

File diff suppressed because it is too large Load Diff

41
src/cli.c
View File

@ -103,6 +103,8 @@ const clivalue_t valueTable[] = {
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "motor_pwm_rate", VAR_UINT16, &cfg.motor_pwm_rate, 50, 498 },
{ "servo_pwm_rate", VAR_UINT16, &cfg.servo_pwm_rate, 50, 498 },
{ "serial_baudrate", VAR_UINT32, &cfg.serial_baudrate, 1200, 115200 },
{ "gps_baudrate", VAR_UINT32, &cfg.gps_baudrate, 1200, 115200 },
{ "spektrum_hires", VAR_UINT8, &cfg.spektrum_hires, 0, 1 },
{ "vbatscale", VAR_UINT8, &cfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 },
@ -127,20 +129,31 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT16, &cfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 },
{ "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
{ "gps_baudrate", VAR_UINT32, &cfg.gps_baudrate, 1200, 115200 },
{ "serial_baudrate", VAR_UINT32, &cfg.serial_baudrate, 1200, 115200 },
{ "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},
{ "p_roll", VAR_UINT8, &cfg.P8[ROLL], 0, 200},
{ "i_roll", VAR_UINT8, &cfg.I8[ROLL], 0, 200},
{ "d_roll", VAR_UINT8, &cfg.D8[ROLL], 0, 200},
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200},
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200},
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200},
{ "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200},
{ "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200},
{ "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200},
{ "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8, &cfg.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8, &cfg.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8, &cfg.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8, &cfg.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8, &cfg.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8, &cfg.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16, &cfg.gps_wp_radius, 0, 2000 },
{ "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 },
{ "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 },
{ "p_roll", VAR_UINT8, &cfg.P8[ROLL], 0, 200 },
{ "i_roll", VAR_UINT8, &cfg.I8[ROLL], 0, 200 },
{ "d_roll", VAR_UINT8, &cfg.D8[ROLL], 0, 200 },
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 },
{ "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200 },
{ "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200 },
{ "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200 },
};
#define VALUE_COUNT (sizeof(valueTable) / sizeof(valueTable[0]))

67
src/config.c
View File

@ -7,13 +7,13 @@
#endif
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage
config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 18;
uint8_t checkNewConf = 19;
void parseRcChannels(const char *input)
{
@ -21,7 +21,7 @@ void parseRcChannels(const char *input)
for (c = input; *c; c++) {
s = strchr(rcChannelLetters, *c);
if (s)
if (s)
cfg.rcmap[s - rcChannelLetters] = c - input;
}
}
@ -31,25 +31,25 @@ void readEEPROM(void)
uint8_t i;
// Read flash
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t));
memcpy(&cfg, (char *) FLASH_WRITE_ADDR, sizeof(config_t));
for(i = 0; i < 6; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500;
for (i = 0; i < 6; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
for(i = 0; i < 11; i++) {
for (i = 0; i < 11; i++) {
int16_t tmp = 10 * i - cfg.thrMid8;
uint8_t y = 1;
if (tmp > 0)
if (tmp > 0)
y = 100 - cfg.thrMid8;
if (tmp < 0)
y = cfg.thrMid8;
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t)cfg.thrExpo8 * (tmp * tmp) / (y * y) ) / 10; // [0;1000]
lookupThrottleRC[i] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle)* lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000]
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
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
}
void writeParams(uint8_t b)
@ -60,12 +60,12 @@ void writeParams(uint8_t b)
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(config_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&cfg + i));
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *) &cfg + i));
if (status != FLASH_COMPLETE)
break; // TODO: fail
break; // TODO: fail
}
}
@ -80,7 +80,7 @@ void checkFirstTime(bool reset)
{
uint8_t test_val, i;
test_val = *(uint8_t *)FLASH_WRITE_ADDR;
test_val = *(uint8_t *) FLASH_WRITE_ADDR;
if (!reset && test_val == checkNewConf)
return;
@ -89,7 +89,7 @@ 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); // | FEATURE_PPM); // sadly, this is for hackers only
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
@ -103,9 +103,15 @@ void checkFirstTime(bool reset)
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDGPS] = 50;
cfg.I8[PIDGPS] = 0;
cfg.D8[PIDGPS] = 15;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
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;
@ -127,12 +133,12 @@ void checkFirstTime(bool reset)
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.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_lpf = 42;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
@ -145,12 +151,12 @@ void checkFirstTime(bool reset)
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
// Motor/ESC/Servo
cfg.minthrottle = 1150;
@ -178,8 +184,13 @@ void checkFirstTime(bool reset)
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps baud-rate
cfg.gps_baudrate = 9600;
// gps/nav stuff
cfg.gps_baudrate = 9600;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial(uart1) baudrate
cfg.serial_baudrate = 115200;

637
src/gps.c
View File

@ -6,100 +6,619 @@
#endif
static void GPS_NewData(uint16_t c);
static bool GPS_newFrame(char c);
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing);
static void GPS_set_pids(void);
void gpsInit(uint32_t baudrate)
{
GPS_set_pids();
uart2Init(baudrate, GPS_NewData);
sensorsSet(SENSOR_GPS);
}
/*-----------------------------------------------------------
*
* Multiwii GPS code
* Multiwii GPS code - revision: 851
*
*-----------------------------------------------------------*/
#define POSHOLD_IMAX 20 // degrees
#define POSHOLD_RATE_IMAX 20 // degrees
#define NAV_IMAX 20 // degrees
static void GPS_NewData(uint16_t c)
/* GPS navigation can control the heading */
#define NAV_TAIL_FIRST 0 // true - copter comes in with tail first
#define NAV_SET_TAKEOFF_HEADING 1 // true - when copter arrives to home position it rotates it's head to takeoff direction
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
static void GPS_calc_poshold(void);
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);
static int32_t wrap_36000(int32_t angle);
typedef struct {
float kP;
float kI;
float kD;
float Imax;
} PID_PARAM;
static PID_PARAM posholdPID;
static PID_PARAM poshold_ratePID;
static PID_PARAM navPID;
// AC_PID.h & AC_PID.cpp
typedef struct {
float _integrator; ///< integrator value
int32_t _last_input; ///< last input for derivative
float _last_derivative; ///< last derivative for low-pass filter
float _output;
float _derivative;
} AC_PID;
static float AC_PID_get_integrator(AC_PID * ac_pid)
{
return ac_pid->_integrator;
}
static void AC_PID_set_integrator(AC_PID * ac_pid, float i)
{
ac_pid->_integrator = i;
}
/// Low pass filter cut frequency for derivative calculation.
// static const float ac_pid_filter = 1.0f / (2.0f * M_PI * (float)cfg.gps_lpf); // Set to "1 / ( 2 * PI * f_cut )"
#define AC_PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
static int32_t AC_PID_get_p(AC_PID * ac_pid, int32_t error, PID_PARAM * pid)
{
return (float) error *pid->kP;
}
static int32_t AC_PID_get_i(AC_PID * ac_pid, int32_t error, float *dt, PID_PARAM * pid)
{
ac_pid->_integrator += ((float) error * pid->kI) * *dt;
if (ac_pid->_integrator < -pid->Imax) {
ac_pid->_integrator = -pid->Imax;
} else if (ac_pid->_integrator > pid->Imax) {
ac_pid->_integrator = pid->Imax;
}
return ac_pid->_integrator;
}
static int32_t AC_PID_get_d(AC_PID * ac_pid, int32_t input, float *dt, PID_PARAM * pid)
{
ac_pid->_derivative = (input - ac_pid->_last_input) / *dt;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
ac_pid->_derivative = ac_pid->_last_derivative + (*dt / (AC_PID_FILTER + *dt)) * (ac_pid->_derivative - ac_pid->_last_derivative);
// update state
ac_pid->_last_input = input;
ac_pid->_last_derivative = ac_pid->_derivative;
// add in derivative component
return pid->kD * ac_pid->_derivative;
}
/// Reset the PID integrator
///
static void AC_PID_reset(AC_PID * ac_pid)
{
ac_pid->_integrator = 0;
ac_pid->_last_input = 0;
ac_pid->_last_derivative = 0;
}
#define _X 1
#define _Y 0
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
static AC_PID pi_poshold[2];
static AC_PID pid_poshold_rate[2];
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)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
//Currently used WP
static int32_t GPS_WP[2];
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
// distance between plane and home in cm
//static int32_t home_distance;
// distance between plane and next_WP in cm
static uint32_t wp_distance;
// used for slow speed wind up when start navigation;
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////////
// moving average filter variables
//
#define GPS_FILTER_VECTOR_LENGTH 5
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c)
{
uint8_t axis;
static uint32_t nav_loopTimer;
uint32_t dist;
int32_t dir;
int16_t speed;
if (GPS_newFrame(c)) {
if (GPS_update == 1)
GPS_update = 0;
else
GPS_update = 1;
if (GPS_fix == 1 && GPS_numSat > 3) {
if (GPS_fix_home == 0) {
GPS_fix_home = 1;
GPS_latitude_home = GPS_latitude;
GPS_longitude_home = GPS_longitude;
if (GPS_fix == 1 && GPS_numSat >= 5) {
if (armed == 0) {
GPS_fix_home = 0;
}
if (GPSModeHold == 1)
GPS_distance(GPS_latitude_hold, GPS_longitude_hold, GPS_latitude, GPS_longitude, &GPS_distanceToHold, &GPS_directionToHold);
else
GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome);
if (GPS_fix_home == 0 && armed) {
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
nav_takeoff_bearing = heading; // save takeoff heading
}
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = ++GPS_filter_index % 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
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999)
GPS_coord[axis] = GPS_filtered[axis];
}
}
#endif
// dTnav calculation
// Time for calculating x,y speed and navigation pids
dTnav = (float) (millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
// calculate distance and bearings for gui and other stuff continously - From home to copter
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
//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
// 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]);
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_poshold();
break;
case NAV_MODE_WP:
speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); //slow navigation
// use error as the desired rate towards the target
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed);
// Tail control
if (cfg.nav_controls_heading) {
if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing - 18000) / 100;
} else {
magHold = nav_bearing / 100;
}
}
// Are we there yet ?(within x meters of the destination)
if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) {
magHold = nav_takeoff_bearing;
}
}
break;
}
} //end of gps calcs
}
}
}
void GPS_reset_home_position(void)
{
GPS_latitude_home = GPS_latitude;
GPS_longitude_home = GPS_longitude;
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
}
/* 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
*/
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing)
//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void)
{
float dLat = (lat2 - lat1); // difference of latitude in 1/100000 degrees
float dLon = (lon2 - lon1) * cosf(lat1 * (M_PI / 180 / 100000.0)); // difference of longitude in 1/100000 degrees
*dist = 6372795.0 / 100000.0 * M_PI / 180.0 * (sqrtf(sq(dLat) + sq(dLon)));
*bearing = 180.0 / M_PI * (atan2f(dLon, dLat));
uint8_t i;
for (i = 0; i < 2; i++) {
GPS_angle[i] = 0;
nav[i] = 0;
AC_PID_reset(&pi_poshold[i]);
AC_PID_reset(&pid_poshold_rate[i]);
AC_PID_reset(&pid_nav[i]);
}
}
/* The latitude or longitude is coded this way in NMEA frames
dm.m coded as degrees + minutes + minute decimal
//Get the relevant P I D values and set the PID controllers
static void GPS_set_pids(void)
{
posholdPID.kP = (float) cfg.P8[PIDPOS] / 100.0f;
posholdPID.kI = (float) cfg.I8[PIDPOS] / 100.0f;
posholdPID.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID.kP = (float) cfg.P8[PIDPOSR] / 10.0f;
poshold_ratePID.kI = (float) cfg.I8[PIDPOSR] / 100.0f;
poshold_ratePID.kD = (float) cfg.D8[PIDPOSR] / 1000.0f;
poshold_ratePID.Imax = POSHOLD_RATE_IMAX * 100;
navPID.kP = (float) cfg.P8[PIDNAVR] / 10.0f;
navPID.kI = (float) cfg.I8[PIDNAVR] / 100.0f;
navPID.kD = (float) cfg.D8[PIDNAVR] / 1000.0f;
navPID.Imax = POSHOLD_RATE_IMAX * 100;
}
// OK here is the onboard GPS code
////////////////////////////////////////////////////////////////////////////////////
// PID based GPS navigation functions
// Author : EOSBandi
// Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
// Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
////////////////////////////////////////////////////////////////////////////////////
// this is used to offset the shrinking longitude as we go towards the poles
// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (abs((float)lat)) * (0.0174532925f / 10000000.0f);
GPS_scaleLonDown = cosf(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
void GPS_set_next_wp(int32_t * lat, int32_t * lon)
{
GPS_WP[LAT] = *lat;
GPS_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat);
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
original_target_bearing = target_bearing;
waypoint_speed_gov = cfg.nav_speed_min;
}
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
static bool check_missed_wp(void)
{
int32_t temp;
temp = target_bearing - original_target_bearing;
temp = wrap_18000(temp);
return (abs(temp) > 10000); // we passed the waypoint by 100 degrees
}
////////////////////////////////////////////////////////////////////////////////////
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing)
{
float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float) (*lon2 - *lon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f;
*bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
////////////////////////////////////////////////////////////////////////////////////
// keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
//
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing) {
// uint32_t d1;
// int32_t d2;
// GPS_distance_cm_bearing(&lat1,&lon1,&lat2,&lon2,&d1,&d2);
// *dist = d1 / 100; //convert to meters
// *bearing = d2 / 100; //convert to degrees
//}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last[2] = { 0, 0 };
static uint8_t init = 0;
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
actual_speed[_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
actual_speed[_X] = (actual_speed[_X] + speed_old[_X]) / 2;
actual_speed[_Y] = (actual_speed[_Y] + speed_old[_Y]) / 2;
speed_old[_X] = actual_speed[_X];
speed_old[_Y] = actual_speed[_Y];
}
init = 1;
last[LON] = GPS_coord[LON];
last[LAT] = GPS_coord[LAT];
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate a location error between two gps coordinates
// Because we are using lat and lon to do our distance errors here's a quick chart:
// 100 = 1m
// 1000 = 11m = 36 feet
// 1800 = 19.80m = 60 feet
// 3000 = 33m
// 10000 = 111m
//
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng)
{
error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LAT] = *target_lat - *gps_lat; // Y Error
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold()
{
int32_t p, i, d;
int32_t output;
int32_t target_speed;
uint8_t 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
rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error
p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis] + error[axis], &dTnav, &poshold_ratePID);
d = AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
d = constrain(d, -2000, 2000);
// get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
if (abs(actual_speed[axis]) < 50)
d = 0;
#endif
output = p + i + d;
nav[axis] = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX);
AC_PID_set_integrator(&pid_nav[axis], AC_PID_get_integrator(&pid_poshold_rate[axis]));
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(int max_speed)
{
float trig[2];
float temp;
uint8_t axis;
// push us towards the original track
GPS_update_crosstrack();
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[_X] = cosf(temp);
trig[_Y] = sinf(temp);
for (axis = 0; axis < 2; axis++) {
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
// P + I + D
nav[axis] = AC_PID_get_p(&pid_nav[axis], rate_error[axis], &navPID)
+ AC_PID_get_i(&pid_nav[axis], rate_error[axis], &dTnav, &navPID)
+ AC_PID_get_d(&pid_nav[axis], rate_error[axis], &dTnav, &navPID);
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
AC_PID_set_integrator(&pid_poshold_rate[axis], AC_PID_get_integrator(&pid_nav[axis]));
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculating cross track error, this tries to keep the copter on a direct line
// when flying to a waypoint.
//
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
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
nav_bearing = target_bearing;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Determine desired speed when navigating towards a waypoint, also implement slow
// speed rampup when starting a navigation
//
// |< WP Radius
// 0 1 2 3 4 5 6 7 8m
// ...|...|...|...|...|...|...|...|
// 100 | 200 300 400cm/s
// | +|+
// |< we should slow to 1.5 m/s as we hit the target
//
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
{
// max_speed is default 400 or 4m/s
if (_slow) {
max_speed = min(max_speed, wp_distance / 2);
max_speed = max(max_speed, 0);
} else {
max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, cfg.nav_speed_min); // go at least 100cm/s
}
// limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command
if (max_speed > waypoint_speed_gov) {
waypoint_speed_gov += (int) (100.0f * dTnav); // increase at .5/ms
max_speed = waypoint_speed_gov;
}
return max_speed;
}
////////////////////////////////////////////////////////////////////////////////////
// Utilities
//
static int32_t wrap_18000(int32_t error)
{
if (error > 18000)
error -= 36000;
if (error < -18000)
error += 36000;
return error;
}
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
angle -= 36000;
if (angle < 0)
angle += 36000;
return angle;
}
// This code is used for parsing NMEA data
/* Alex optimization
The latitude or longitude is coded this way in NMEA frames
dm.f 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
- f can be 1 or more char long
This function converts this format in a unique unsigned long where 1 degree = 10 000 000
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, *d = s;
uint32_t sec, m = 1000;
uint16_t min, dec = 0;
char *p = s, *d = s;
uint8_t min, deg = 0;
uint16_t frac = 0, mult = 10000;
if (!*s)
return 0;
for (p = s; *p != 0; p++) {
while (*p) { // parse the string until its end
if (d != s) {
*p -= '0';
dec += *p * m;
m /= 10;
frac += (*p - '0') * mult; // calculate only fractional part on up to 5 digits (d != s condition is true when the . is located)
mult /= 10;
}
if (*p == '.')
d = p;
d = p; // locate '.' char in the string
p++;
}
m = 10000;
min = *--d - '0';
min += (*--d - '0') * 10;
sec = (m * min + dec) / 6;
while (d != s) {
m *= 10;
*--d -= '0';
sec += *d * m;
if (p == s)
return 0;
while (s < d - 2) {
deg *= 10; // convert degrees : all chars before minutes ; for the first iteration, deg = 0
deg += *(s++) - '0';
}
return sec;
min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
}
// helper functions
// helper functions
static uint16_t grab_fields(char *src, uint8_t mult)
{ // convert string to uint16
uint8_t i;
@ -164,24 +683,24 @@ static bool GPS_newFrame(char c)
frame = FRAME_RMC;
} else if (frame == FRAME_GGA) {
if (param == 2) {
GPS_latitude = GPS_coord_to_degrees(string);
GPS_coord[LAT] = GPS_coord_to_degrees(string);
} else if (param == 3 && string[0] == 'S')
GPS_latitude = -GPS_latitude;
GPS_coord[LAT] = -GPS_coord[LAT];
else if (param == 4) {
GPS_longitude = GPS_coord_to_degrees(string);
GPS_coord[LON] = GPS_coord_to_degrees(string);
} else if (param == 5 && string[0] == 'W')
GPS_longitude = -GPS_longitude;
GPS_coord[LON] = -GPS_coord[LON];
else if (param == 6) {
GPS_fix = string[0] > '0';
} else if (param == 7) {
GPS_numSat = grab_fields(string, 0);
} else if (param == 9) {
GPS_altitude = grab_fields(string, 0);
} // altitude in meters added by Mis
GPS_altitude = grab_fields(string, 0); // altitude in meters added by Mis
}
} 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) * 514444L) / 100000L; // speed in cm/s added by Mis
}
}
param++;
offset = 0;
@ -204,5 +723,7 @@ static bool GPS_newFrame(char c)
if (!checksum_param)
parity ^= c;
}
if (frame)
GPS_Present = 1;
return frameOK && (frame == FRAME_GGA);
}

117
src/mw.c
View File

@ -3,12 +3,9 @@
// March 2012 V2.0
#define CHECKBOXITEMS 11
#define PIDITEMS 8
int16_t debug1, debug2, debug3, debug4;
uint8_t buzzerState = 0;
uint8_t toggleBeep = 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
@ -24,11 +21,11 @@ uint8_t vbat; // battery voltage in 0.1V steps
int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t rcOptions[CHECKBOXITEMS];
@ -42,19 +39,21 @@ int16_t axisPID[3];
// **********************
// GPS
// **********************
int32_t GPS_latitude, GPS_longitude;
int32_t GPS_latitude_home, GPS_longitude_home;
int32_t GPS_latitude_hold, GPS_longitude_hold;
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; // in meters
int16_t GPS_directionToHome = 0; // in degrees
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_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
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
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];
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
//Automatic ACC Offset Calibration
// **********************
@ -89,7 +88,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp,tmp2;
uint16_t tmp, tmp2;
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring
uint8_t axis, prop1, prop2;
@ -103,7 +102,7 @@ void annexCode(void)
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
} else {
prop2 = 100 - cfg.dynThrPID;
}
@ -119,11 +118,11 @@ void annexCode(void)
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
@ -133,18 +132,18 @@ void annexCode(void)
}
}
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t)cfg.yawRate * tmp / 500;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t)(tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
// rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck);
if (headFreeMode) {
@ -165,7 +164,7 @@ void annexCode(void)
}
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
buzzerFreq = 7;
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
buzzerFreq = 0;
buzzerState = 0;
BEEP_OFF;
@ -173,7 +172,7 @@ void annexCode(void)
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
@ -209,7 +208,7 @@ void annexCode(void)
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
if (currentTime > GPSLEDTime && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
@ -276,16 +275,16 @@ void loop(void)
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
computeRC();
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) { // 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
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
}
failsafeEvents++;
}
@ -326,7 +325,7 @@ void loop(void)
} else if (armed)
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)) && 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] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
@ -396,11 +395,8 @@ void loop(void)
}
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;
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;
}
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
@ -457,14 +453,20 @@ void loop(void)
if (sensors(SENSOR_GPS)) {
if (rcOptions[BOXGPSHOME]) {
GPSModeHome = 1;
if (GPSModeHome == 0) {
GPSModeHome = 1;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else
GPSModeHome = 0;
if (rcOptions[BOXGPSHOLD]) {
if (GPSModeHold == 0) {
GPSModeHold = 1;
GPS_latitude_hold = GPS_latitude;
GPS_longitude_hold = GPS_longitude;
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;
@ -545,44 +547,33 @@ void loop(void)
#endif
if (sensors(SENSOR_GPS)) {
uint16_t GPS_dist = 0;
int16_t GPS_dir = 0;
// Check that we really need to navigate ?
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
float radDiff;
if (GPSModeHome == 1) {
GPS_dist = GPS_distanceToHome;
GPS_dir = GPS_directionToHome;
}
if (GPSModeHold == 1) {
GPS_dist = GPS_distanceToHold;
GPS_dir = GPS_directionToHold;
}
radDiff = (GPS_dir - heading) * M_PI / 180.0f;
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sinf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cosf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
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 (accMode == 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
PTerm = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTerm = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTerm = (int32_t) errorAngle *cfg.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, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp // 16 bits is ok here
ITerm = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ((int32_t) errorAngleI[axis] * cfg.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
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.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 = (int32_t) rcCommand[axis] * 10 * 8 / cfg.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];
@ -592,7 +583,7 @@ void loop(void)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
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];
@ -600,7 +591,7 @@ void loop(void)
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}

70
src/mw.h
View File

@ -24,6 +24,18 @@
#define VERSION 200
#define LAT 0
#define LON 1
// Serial GPS only variables
// navigation mode
typedef enum NavigationMode
{
NAV_MODE_NONE = 1,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} NavigationMode;
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
{
@ -64,10 +76,12 @@ typedef enum GimbalFlags {
#define AUX4 7
#define PIDALT 3
#define PIDVEL 4
#define PIDGPS 5
#define PIDLEVEL 6
#define PIDMAG 7
#define PIDPOS 4
#define PIDPOSR 5
#define PIDNAVR 6
#define PIDLEVEL 7
#define PIDMAG 8
#define PIDVEL 9 // not used currently
#define BOXACC 0
#define BOXBARO 1
@ -82,7 +96,7 @@ typedef enum GimbalFlags {
#define BOXBEEPERON 10
#define CHECKBOXITEMS 11
#define PIDITEMS 8
#define PIDITEMS 10
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
@ -94,9 +108,9 @@ typedef struct config_t {
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint8_t P8[8];
uint8_t I8[8];
uint8_t D8[8];
uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS];
uint8_t rcRate8;
uint8_t rcExpo8;
@ -165,8 +179,13 @@ typedef struct config_t {
uint16_t gimbal_roll_max; // gimbal roll servo max travel
uint16_t gimbal_roll_mid; // gimbal roll servo neutral value
// gps baud-rate
uint32_t gps_baudrate;
// gps-related stuff
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_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
// serial(uart1) baudrate
uint32_t serial_baudrate;
@ -212,18 +231,23 @@ extern int16_t rcData[8];
extern uint8_t accMode;
extern uint8_t magMode;
extern uint8_t baroMode;
extern int32_t GPS_latitude, GPS_longitude;
extern int32_t GPS_latitude_home, GPS_longitude_home;
extern int32_t GPS_latitude_hold, GPS_longitude_hold;
extern uint8_t GPS_fix, GPS_fix_home;
extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome, GPS_distanceToHold;
extern int16_t GPS_directionToHome, GPS_directionToHold;
extern uint8_t GPS_update;
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
extern uint16_t GPS_altitude;
extern uint16_t GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
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
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 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
@ -292,3 +316,5 @@ void cliProcess(void);
// gps
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);

220
src/serial.c
View File

@ -1,35 +1,36 @@
#include "board.h"
#include "mw.h"
#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
#define MSP_SERVO 103 //out message 8 servos
#define MSP_MOTOR 104 //out message 8 motors
#define MSP_RC 105 //out message 8 rc chan
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message 1 altitude
#define MSP_BAT 110 //out message vbat, powermetersum
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message up to 16 P I D (8 are used)
#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_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
#define MSP_SERVO 103 //out message 8 servos
#define MSP_MOTOR 104 //out message 8 motors
#define MSP_RC 105 //out message 8 rc chan
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message 1 altitude
#define MSP_BAT 110 //out message vbat, powermetersum
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message up to 16 P I D (8 are used)
#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_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID 202 //in message up to 16 P I D (8 are used)
#define MSP_SET_BOX 203 //in message up to 16 checkbox (11 are used)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_ACC_CALIBRATION 205 //in message no param
#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_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID 202 //in message up to 16 P I D (8 are used)
#define MSP_SET_BOX 203 //in message up to 16 checkbox (11 are used)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_ACC_CALIBRATION 205 //in message no param
#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_EEPROM_WRITE 250 //in message no param
#define MSP_EEPROM_WRITE 250 //in message no param
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
static uint8_t checksum, stateMSP, indRX, inBuf[64];
@ -70,16 +71,16 @@ void serialize8(uint8_t 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;
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;
t += inBuf[indRX++] << 8;
return t;
}
@ -143,8 +144,8 @@ void serialCom(void)
case MSP_SET_RAW_GPS:
GPS_fix = read8();
GPS_numSat = read8();
GPS_latitude = read32();
GPS_longitude = read32();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update = 1;
@ -200,7 +201,7 @@ void serialCom(void)
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
serialize8(cfg.mixerConfiguration); // the second one
tailSerialReply();
break; // mainly to send the last octet which is the checksum
case MSP_STATUS:
@ -243,8 +244,8 @@ void serialCom(void)
headSerialReply(c, 14);
serialize8(GPS_fix);
serialize8(GPS_numSat);
serialize32(GPS_latitude);
serialize32(GPS_longitude);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
tailSerialReply();
@ -306,6 +307,12 @@ void serialCom(void)
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;
@ -351,148 +358,7 @@ void serialCom(void)
if (c == '#')
cliProcess();
else if (c == 'R')
systemReset(true); // reboot to bootloader
systemReset(true); // reboot to bootloader
}
}
}
#if 0
void oldSserialCom(void)
{
uint8_t i;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
if (uartAvailable()) {
switch (uartRead()) {
case '#':
cliProcess();
break;
#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
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 / 10);
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(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4 | armed << 5);
#if defined(LOG_VALUES)
serialize16(cycleTimeMax);
cycleTimeMax = 0;
#else
serialize16(cycleTime);
#endif
serialize16(i2cGetErrorCounter());
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize8(cfg.mixerConfiguration);
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
for (i = 0; i < CHECKBOXITEMS; i++)
serialize16(cfg.activate[i]);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome + 180);
serialize8(GPS_numSat);
serialize8(GPS_fix);
serialize8(GPS_update);
serialize16(0); // power meter, removed
serialize16(0); // power meter, removed
serialize8(vbat);
serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose
serialize16(debug2); // debug2
serialize16(debug3); // debug3
serialize16(debug4); // debug4
serialize8('M');
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(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4 | armed << 5);
serialize8(vbat); // Vbatt 47
serialize8(VERSION); // MultiWii Firmware version
serialize8(GPS_fix); // Fix indicator for OSD
serialize8(GPS_numSat);
serialize16(GPS_latitude);
serialize16(GPS_latitude >> 16);
serialize16(GPS_longitude);
serialize16(GPS_longitude >> 16);
serialize16(GPS_altitude);
serialize16(GPS_speed); // Speed for OSD
serialize8('O'); // NOT 49 anymore
break;
case 'R': // reboot to bootloader (oops, apparently this w as used for other trash, fix later)
systemReset(true);
break;
case 'W': //GUI write params to eeprom @ arduino
// while (uartAvailable() < (7 + 3 * PIDITEMS + 2 * CHECKBOXITEMS)) { }
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = uartReadPoll();
cfg.I8[i] = uartReadPoll();
cfg.D8[i] = uartReadPoll();
}
cfg.rcRate8 = uartReadPoll();
cfg.rcExpo8 = uartReadPoll(); //2
cfg.rollPitchRate = uartReadPoll();
cfg.yawRate = uartReadPoll(); //4
cfg.dynThrPID = uartReadPoll(); //5
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = uartReadPoll();
uartReadPoll(); // power meter crap, removed
uartReadPoll(); // power meter crap, removed
writeParams(0);
break;
case 'S': // GUI to arduino ACC calibration request
calibratingA = 400;
break;
case 'E': // GUI to arduino MAG calibration request
calibratingM = 1;
break;
}
}
}
#endif