Skip to content

Commit

Permalink
- consider I2C_GPS as a simple GPS device like SERIAL GPS: the same c…
Browse files Browse the repository at this point in the history 
…ode is now used for all GPS devices. (best option to keep old I2C GPS compatibility, but code size with GPS is now an issue with pro micro proc)

- make HEADFREE optional
- use struct to serialize GPS data in MSP part

git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii_shared@1666 02679b44-d973-9852-f2fa-63770883b36c
  • Loading branch information
dubusal committed Mar 21, 2014
1 parent af8945c commit e3251cb
Show file tree
Hide file tree
Showing 7 changed files with 236 additions and 387 deletions.
401 changes: 115 additions & 286 deletions GPS.cpp
View file

Large diffs are not rendered by default.

3 changes: 0 additions & 3 deletions GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,5 @@ uint8_t GPS_Compute(void);
void GPS_reset_home_position(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void GPS_reset_nav(void);
#if defined(I2C_GPS)
void GPS_I2C_command(uint8_t command, uint8_t wp);
#endif

#endif /* GPS_H_ */
8 changes: 5 additions & 3 deletions LCD.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1220,7 +1220,7 @@ PROGMEM const void * const lcd_param_ptr_table [] = {
&lcd_param_text50, &conf.activate[BOXPASSTHRU],&__AUX4,
#endif
#endif
#if MAG
#if MAG && defined(HEADFREE)
&lcd_param_text51, &conf.activate[BOXHEADFREE],&__AUX1,
#ifndef SUPPRESS_LCD_CONF_AUX2
&lcd_param_text51, &conf.activate[BOXHEADFREE],&__AUX2,
Expand Down Expand Up @@ -1839,8 +1839,10 @@ static char checkboxitemNames[][4] = {
#endif
#if MAG
"Mag",
"HFr",
"HAd",
#if defined(HEADFREE)
"HFr",
"HAd",
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
"CSt",
Expand Down
127 changes: 63 additions & 64 deletions MultiWii.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,10 @@ const char boxnames[] PROGMEM = // names for dynamic generation of config GUI
#endif
#if MAG
"MAG;"
"HEADFREE;"
"HEADADJ;"
#if defined(HEADFREE)
"HEADFREE;"
"HEADADJ;"
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
"CAMSTAB;"
Expand Down Expand Up @@ -108,8 +110,10 @@ const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way
#endif
#if MAG
5, //"MAG;"
6, //"HEADFREE;"
7, //"HEADADJ;"
#if defined(HEADFREE)
6, //"HEADFREE;"
7, //"HEADADJ;"
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
8, //"CAMSTAB;"
Expand Down Expand Up @@ -367,14 +371,16 @@ void annexCode() { // this code is excetuted at each loop and won't interfere wi
tmp2 = tmp/256; // range [0;9]
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTLE]

if(f.HEADFREE_MODE) { //to optimize
float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
float cosDiff = cos(radDiff);
float sinDiff = sin(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#if defined(HEADFREE)
if(f.HEADFREE_MODE) { //to optimize
float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
float cosDiff = cos(radDiff);
float sinDiff = sin(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#endif

// query at most one multiplexed analog channel per MWii cycle
static uint8_t analogReader =0;
Expand Down Expand Up @@ -620,7 +626,7 @@ void setup() {
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
#if GPS
GPS_set_pids();
#endif
previousTime = micros();
Expand All @@ -633,29 +639,28 @@ void setup() {
for(uint8_t j=0; j<=PMOTOR_SUM; j++) pMeter[j]=0;
#endif
/************************************/
#if defined(GPS_SERIAL)
GPS_SerialInit();
for(uint8_t j=0;j<=5;j++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
#if defined(GPS_PROMINI)
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL0_COM_SPEED);
#if GPS
#if defined(GPS_SERIAL)
GPS_SerialInit();
for(uint8_t j=0;j<=5;j++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
#if defined(GPS_PROMINI)
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL0_COM_SPEED);
}
#else
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#else
GPS_Present = 1;
GPS_Enable = 1;
#endif
GPS_Enable = GPS_Present;
#endif
/************************************/

#if defined(I2C_GPS) || defined(GPS_FROM_OSD)
GPS_Enable = 1;
#endif

#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(OLED_DIGOLE) || defined(LCD_TELEMETRY_STEP)
Expand Down Expand Up @@ -705,7 +710,9 @@ void go_arm() {
) {
if(!f.ARMED && !f.BARO_MODE) { // arm now!
f.ARMED = 1;
headFreeModeHold = att.heading;
#if defined(HEADFREE)
headFreeModeHold = att.heading;
#endif
magHold = att.heading;
#if defined(VBAT)
if (analog.vbat > NO_VBAT) vbatMin = analog.vbat;
Expand Down Expand Up @@ -1027,21 +1034,23 @@ void loop () {
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
#if defined(HEADFREE)
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
#if defined(ADVANCED_HEADFREE)
if ((f.GPS_FIX && GPS_numSat >= 5) && (GPS_distanceToHome > ADV_HEADFREE_RANGE) ) {
if (GPS_directionToHome < 180) {headFreeModeHold = GPS_directionToHome + 180;} else {headFreeModeHold = GPS_directionToHome - 180;}
}
#endif
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = att.heading; // acquire new heading
}
#if defined(ADVANCED_HEADFREE)
if ((f.GPS_FIX && GPS_numSat >= 5) && (GPS_distanceToHome > ADV_HEADFREE_RANGE) ) {
if (GPS_directionToHome < 180) {headFreeModeHold = GPS_directionToHome + 180;} else {headFreeModeHold = GPS_directionToHome - 180;}
}
#endif
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = att.heading; // acquire new heading
}
#endif

#if GPS
Expand All @@ -1052,27 +1061,19 @@ void loop () {
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
#else // SERIAL
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
#endif
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
#else
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;
#endif
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 {
f.GPS_HOLD_MODE = 0;
Expand All @@ -1086,9 +1087,7 @@ void loop () {
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
#if !defined(I2C_GPS)
nav_mode = NAV_MODE_NONE;
#endif
nav_mode = NAV_MODE_NONE;
}
#endif

Expand Down
75 changes: 46 additions & 29 deletions Protocol.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,7 +236,7 @@ void s_struct(uint8_t *cb,uint8_t siz) {
}

void s_struct_w(uint8_t *cb,uint8_t siz) {
headSerialReply(0);
headSerialReply(0);
while(siz--) *cb++ = read8();
}

Expand All @@ -252,16 +252,22 @@ void evaluateCommand() {
s_struct_w((uint8_t*)&rcSerial,16);
rcSerialCount = 50; // 1s transition
break;
#if GPS
#if GPS && !defined(I2C_GPS)
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();
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f;
} set_set_raw_gps;
s_struct_w((uint8_t*)&set_set_raw_gps,14);
f.GPS_FIX = set_set_raw_gps.a;
GPS_numSat = set_set_raw_gps.b;
GPS_coord[LAT] = set_set_raw_gps.c;
GPS_coord[LON] = set_set_raw_gps.d;
GPS_altitude = set_set_raw_gps.e;
GPS_speed = set_set_raw_gps.f;
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
#endif
case MSP_SET_PID:
Expand Down Expand Up @@ -386,8 +392,10 @@ void evaluateCommand() {
#if MAG
if(f.MAG_MODE) tmp |= 1<<BOXMAG;
#if !defined(FIXEDWING)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#if defined(HEADFREE)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
Expand Down Expand Up @@ -459,20 +467,31 @@ void evaluateCommand() {
break;
#if GPS
case MSP_RAW_GPS:
headSerialReply(16);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
serialize16(GPS_ground_course); // added since r1172
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f,g;
} msp_raw_gps;
msp_raw_gps.a = f.GPS_FIX;
msp_raw_gps.b = GPS_numSat;
msp_raw_gps.c = GPS_coord[LAT];
msp_raw_gps.d = GPS_coord[LON];
msp_raw_gps.e = GPS_altitude;
msp_raw_gps.f = GPS_speed;
msp_raw_gps.g = GPS_ground_course;
s_struct((uint8_t*)&msp_raw_gps,16);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
struct {
uint16_t a;
int16_t b;
uint8_t c;
} msp_comp_gps;
msp_comp_gps.a = GPS_distanceToHome;
msp_comp_gps.b = GPS_directionToHome;
msp_comp_gps.c = GPS_update & 1;
s_struct((uint8_t*)&msp_comp_gps,5);
break;
#endif
case MSP_ATTITUDE:
Expand Down Expand Up @@ -543,17 +562,15 @@ void evaluateCommand() {
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop
f.GPS_FIX_HOME = 1;
if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
} else if (wp_no == 16) {
GPS_hold[LAT] = lat;
GPS_hold[LON] = lon;
if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps
#if !defined(I2C_GPS)
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
#endif
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
}
}
headSerialReply(0);
Expand Down
3 changes: 3 additions & 0 deletions config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -536,6 +536,9 @@

//#define THROTTLE_ANGLE_CORRECTION 40

/*** HEADFREE : the copter can be controled by an absolute stick orientation, whatever the yaw orientation ***/
//#define HEADFREE

/************************* Advanced Headfree Mode ********************/
/* In Advanced Headfree mode when the copter is farther than ADV_HEADFREE_RANGE meters then
the bearing between home and copter position will become the control direction
Expand Down
6 changes: 4 additions & 2 deletions types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,10 @@ enum box {
#endif
#if MAG
BOXMAG,
BOXHEADFREE,
BOXHEADADJ, // acquire heading for HEADFREE mode
#if defined(HEADFREE)
BOXHEADFREE,
BOXHEADADJ, // acquire heading for HEADFREE mode
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(SERVO_MIX_TILT)
BOXCAMSTAB,
Expand Down

0 comments on commit e3251cb

Please sign in to comment.