Merge pull request #28 from raul-ortega/development

Development

src/main/config/config.c

@@ -717,6 +717,7 @@ static void resetConf(void)
     //TILT
     masterConfig.tilt0 = 500;
     masterConfig.tilt90 = 1400;
+    masterConfig.tilt_max_angle = 0;
 
     //EASING
     featureSet(FEATURE_EASING);

src/main/config/config_master.h

@@ -120,6 +120,7 @@ typedef struct master_t {
     int8_t offset_trim;
 	uint16_t tilt0;
 	uint16_t tilt90;
+	uint8_t tilt_max_angle;
 	uint8_t easing;
 	uint8_t easing_steps;
 	uint8_t easing_min_angle;

src/main/io/display.c

@@ -71,6 +71,7 @@ extern int32_t telemetry_lon;
 extern int16_t telemetry_alt;
 extern int16_t telemetry_sats;
 extern uint8_t telemetry_failed_cs;
+extern uint8_t telemetry_fixtype;
 extern positionVector_t targetPosition;
 extern positionVector_t trackerPosition;
 extern bool gotFix;
@@ -264,6 +265,18 @@ static const char* const epsParamIncreaseDecrease[] = {
 	"EXIT         "
 };
 
+static const char* const telemetryFixType[] = {
+	"none ",
+	"nofix",
+	"2d   ",
+	"3d   ",
+	"dgps ",
+	"float",
+	"fixed",
+	"stati",
+	"ppp  "
+};
+
 uint8_t indexMenuOption = 0;
 uint8_t maxMenuOptions = 5;
 uint8_t menuState = 0;
@@ -644,10 +657,8 @@ void showTelemetryPage(void){
     if(!PROTOCOL(TP_MFD)) {
     	if(telemetry_sats>99)
     		telemetry_sats = 99;
-    	//if(PROTOCOL(TP_GPS_TELEMETRY))
-    	tfp_sprintf(lineBuffer, "Sats: %02d     FCS:%d", telemetry_sats,telemetry_failed_cs);
-    	//else
-    		//tfp_sprintf(lineBuffer, "Sats: %02d  ", telemetry_sats);
+
+    	tfp_sprintf(lineBuffer, "Sat: %02d %s FCS:%d", telemetry_sats, telemetryFixType[telemetry_fixtype], telemetry_failed_cs);
 		padLineBuffer();
 		i2c_OLED_set_line(rowIndex++);
 		i2c_OLED_send_string(lineBuffer);
@@ -768,12 +779,12 @@ void showGpsPage() {
 
 
     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
-    tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
+    tfp_sprintf(lineBuffer, "Sat: %d Fix: %c", GPS_numSat, fixChar);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
+    tfp_sprintf(lineBuffer, "La/Lo: %d/%d Hdop:%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER, GPS_hdop);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);

src/main/io/serial_cli.c

@@ -569,8 +569,8 @@ const clivalue_t valueTable[] = {
 /*
     { "align_gyro",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
-    { "align_mag",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
-
+*/  { "align_mag",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
+/*
     { "align_board_roll",           VAR_INT16  | MASTER_VALUE,  &masterConfig.boardAlignment.rollDegrees, .config.minmax = { -180,  360 } },
     { "align_board_pitch",          VAR_INT16  | MASTER_VALUE,  &masterConfig.boardAlignment.pitchDegrees, .config.minmax = { -180,  360 } },
     { "align_board_yaw",            VAR_INT16  | MASTER_VALUE,  &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180,  360 } },
@@ -723,6 +723,7 @@ const clivalue_t valueTable[] = {
 	{ "init_servos",    			VAR_UINT8  | TRACKER_VALUE, &masterConfig.init_servos, .config.minmax = { 0,  1 } },
 	{ "tilt0",           			VAR_UINT16  | TRACKER_VALUE, &masterConfig.tilt0, .config.minmax = { 0,  3000 } },
 	{ "tilt90",           			VAR_UINT16  | TRACKER_VALUE, &masterConfig.tilt90, .config.minmax = { 0,  3000 } },
+	{ "tilt_max_angle",         	VAR_UINT8  | TRACKER_VALUE, &masterConfig.tilt_max_angle, .config.minmax = { 0,  90 } },
 	{ "easing",           			VAR_UINT8  | TRACKER_VALUE, &masterConfig.easing, .config.minmax = { 1,  4 } }, //0: no easing, 1: out-quart function, 2: out-circ function
 	{ "easing_steps",           	VAR_UINT8  | TRACKER_VALUE, &masterConfig.easing_steps, .config.minmax = { 0,  100 } },
 	{ "easing_min_angle",           VAR_UINT8  | TRACKER_VALUE, &masterConfig.easing_min_angle, .config.minmax = { 1,  10 } },
@@ -2480,7 +2481,7 @@ static void cliMovePanServo(char *cmdline)
     } else {
     	degrees=atoi(cmdline);
     	if(degrees>=0 && degrees<=360 ) {
-			printf("Moving pan servo to %u º\r\n", degrees);
+			printf("Moving pan servo to %u º\r\n", degrees);
 			SERVOTEST_HEADING=degrees;
 			ENABLE_SERVO(SERVOPAN_MOVE);
     	}
@@ -2499,7 +2500,7 @@ static void cliMoveTiltServo(char *cmdline)
     } else {
     	degrees=atoi(cmdline);
     	if(degrees >=0 && degrees <= 90 ) {
-			printf("Moving tilt servo to %u º\r\n", degrees);
+			printf("Moving tilt servo to %u º\r\n", degrees);
 			SERVOTEST_TILT = degrees;
 			ENABLE_SERVO(SERVOTILT_MOVE);
     	}

src/main/target/SPRACINGF3/target.h

@@ -156,8 +156,8 @@
 #define WS2811_DMA_CHANNEL              DMA1_Channel2
 #define WS2811_IRQ                      DMA1_Channel2_IRQn
 
-#define BLACKBOX
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+//#define BLACKBOX
+//#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 
 #define DISPLAY
 #define GPS

src/main/tracker/TinyGPS.c

@@ -197,6 +197,7 @@ bool term_complete()
           _time      = _new_time;
           _latitude  = _new_latitude;
           _longitude = _new_longitude;
+          _fixtype = _new_fixtype;
           _numsats   = _new_numsats;
           _hdop      = _new_hdop;
           break;
@@ -274,6 +275,7 @@ bool term_complete()
     case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
     case COMBINE(_GPS_SENTENCE_GNGGA, 6):
       _gps_data_good = _term[0] > '0';
+      _new_fixtype = (unsigned char)atoi(_term);
       break;
     case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
     case COMBINE(_GPS_SENTENCE_GNGGA, 7):
@@ -388,6 +390,10 @@ void f_get_position(float *latitude, float *longitude, unsigned long *fix_age)
   *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 1000000.0);
 }
 
+uint8_t f_fixtype(void){
+	return _fixtype;
+}
+
 void crack_datetime(int *year, uint8_t *month, uint8_t *day,
   uint8_t *hour, uint8_t *minute, uint8_t *second, uint8_t *hundredths, unsigned long *age)
 {
@@ -406,42 +412,42 @@ void crack_datetime(int *year, uint8_t *month, uint8_t *day,
   if (hundredths) *hundredths = time % 100;
 }
 
-float f_altitude()
+float f_altitude(void)
 {
   return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0;
 }
 
-float f_course()
+float f_course(void)
 {
   return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0;
 }
 
-float f_speed_knots()
+float f_speed_knots(void)
 {
   return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0;
 }
 
-float f_speed_mph()
+float f_speed_mph(void)
 {
   float sk = f_speed_knots();
   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * sk;
 }
 
-float f_speed_mps()
+float f_speed_mps(void)
 {
   float sk = f_speed_knots();
   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * sk;
 }
 
-float f_speed_kmph()
+float f_speed_kmph(void)
 {
   float sk = f_speed_knots();
   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * sk;
 }
 
-float f_hdop()
+float f_hdop(void)
 {
-	_speed == GPS_INVALID_HDOP ? GPS_INVALID_HDOP : _hdop / 100.0;
+	_hdop == GPS_INVALID_HDOP ? GPS_INVALID_HDOP : _hdop / 100.0;
 }
 
 bool gpsisdigit(char c) {

src/main/tracker/TinyGPS.h

@@ -49,6 +49,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
   unsigned long _date, _new_date;
   long _latitude, _new_latitude;
   long _longitude, _new_longitude;
+  unsigned short _fixtype, _new_fixtype;
   long _altitude, _new_altitude;
   unsigned long  _speed, _new_speed;
   unsigned long  _course, _new_course;
@@ -66,6 +67,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
   uint8_t _term_number;
   uint8_t _term_offset;
   bool _gps_data_good;
+  uint8_t _fix_type;
 
 #ifndef _GPS_NO_STATS
   // statistics
@@ -104,13 +106,14 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
   void f_get_position(float *latitude, float *longitude, unsigned long *fix_age);
   void crack_datetime(int *year, uint8_t *month, uint8_t *day,
     uint8_t *hour, uint8_t *minute, uint8_t *second, uint8_t *hundredths, unsigned long *fix_age);
-  float f_altitude();
-  float f_course();
-  float f_speed_knots();
-  float f_speed_mph();
-  float f_speed_mps();
-  float f_speed_kmph();
-  float f_hdop();
+  float f_altitude(void);
+  float f_course(void);
+  float f_speed_knots(void);
+  float f_speed_mph(void);
+  float f_speed_mps(void);
+  float f_speed_kmph(void);
+  float f_hdop(void);
+  uint8_t f_fixtype(void);
 
   uint8_t get_sentence_type(void);
   //static int library_version() { return _GPS_VERSION; }

src/main/tracker/ltm.c

@@ -159,7 +159,7 @@ void parseLTM_GFRAME(void) {
     telemetry_alt = (int16_t)(temp_alt/100);
     satsfix = ltmread_u8();
     telemetry_sats = (int16_t)((satsfix >> 2) & 0xFF);
-    fix_type = satsfix & 0b00000011;
+    telemetry_fixtype = satsfix & 0b00000011;
     if(telemetry_sats>=5) gotFix = 1;
     gotAlt = true;
   }

src/main/tracker/main.c

@@ -152,6 +152,7 @@ void protocolInit(void);
 void trackingInit(void);
 void telemetryPortInit(void);
 void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep);
+uint8_t filterTiltAngle(uint8_t target);
 //EASING
 int16_t _lastTilt;
 int16_t tilt;
@@ -495,6 +496,8 @@ void calcTilt(void) {
   else if (tiltTarget > 90)
     tiltTarget = 90;
 
+  tiltTarget = filterTiltAngle(tiltTarget);
+
   if(feature(FEATURE_EASING)) {
 	if(_servo_tilt_has_arrived){
 		_servo_tilt_must_move = tiltTarget;
@@ -755,6 +758,7 @@ void updateServoTest(void){
 		 }
 
 		 if(SERVO(SERVOTILT_MOVE)) {
+			SERVOTEST_TILT = filterTiltAngle(SERVOTEST_TILT);
 			if(feature(FEATURE_EASING) && masterConfig.easing > 0) {//#ifdef TILT_EASING
 				  _servo_tilt_must_move = SERVOTEST_TILT;
 				  _servo_tilt_has_arrived = false;
@@ -1592,3 +1596,7 @@ void protocolInit(void){
 		break;
 	}
 }
+
+uint8_t filterTiltAngle(uint8_t target){
+	return (masterConfig.tilt_max_angle > 0 && target > masterConfig.tilt_max_angle)?masterConfig.tilt_max_angle:target;
+}

src/main/tracker/mavlink.c

@@ -33,10 +33,11 @@ void mavlink_handleMessage(mavlink_message_t* msg) {
       telemetry_lon = mavlink_msg_gps_raw_int_get_lon(msg) / 10;
       telemetry_sats = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
       telemetry_alt = (int16_t)(mavlink_msg_gps_raw_int_get_alt(msg) / 1000);
+      telemetry_fixtype = mavlink_msg_gps_raw_int_get_fix_type(msg);
       gotAlt = true;
 
       // fix_type: GPS lock 0-1=no fix, 2=2D, 3=3D
-      if (mavlink_msg_gps_raw_int_get_fix_type(msg) > 1) {
+      if (telemetry_fixtype > 1) {
         gotFix = true;
       } else {
         gotFix = false;

src/main/tracker/telemetry.c

@@ -33,6 +33,7 @@ int32_t telemetry_time = 0;
 int32_t telemetry_date = 0;
 int16_t telemetry_age = 0;
 
+
 uint8_t telemetry_failed_cs = 0;
 
 float telemetry_course = 0.0f;
@@ -46,6 +47,8 @@ float telemetry_yaw = 0.0f;
 
 uint8_t telemetry_provider = 0;
 
+uint8_t telemetry_fixtype = 0;
+
 uint8_t a;
 
 uint8_t LOCAL_GPS;
@@ -125,6 +128,8 @@ void gps_encodeTargetData(uint8_t c) {
       //hdop
       telemetry_hdop = f_hdop();
 
+      telemetry_fixtype = f_fixtype();
+
       if(!LOCAL_GPS){
         if (satellites() != GPS_INVALID_SATELLITES) {
         	telemetry_sats = (int16_t)satellites();

src/main/tracker/telemetry.h

@@ -47,6 +47,7 @@ extern float telemetry_roll;
 extern float telemetry_yaw;
 extern uint8_t telemetry_failed_cs;
 extern uint8_t telemetry_provider;
+extern uint8_t telemetry_fixtype;
 
 int16_t getTargetAlt(int16_t home_alt);
 void encodeTargetData(uint8_t c);

src/main/version.h

@@ -16,7 +16,7 @@
  */
 
 #define FC_VERSION_MAJOR            9 // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            3 // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            4 // increment when a minor release is made (small new feature, change etc)
 #define FC_VERSION_PATCH_LEVEL      0 // increment when a bug is fixed
 
 #define STR_HELPER(x) #x