Teal One airframe improvements for full support (#10860)

* added a parameter for enabled the smart battery
* start tel1 and tel2 at 921600 and max rate
* turned up the max tx rate on mavlink streams for telem1 and telem2
* turned off mavlink stream for tel1 in 4250_teal. This is connected to TX1 FTDI UART and has issues.
* moved check to 4250 board ID to start mpu9250s for Teal airframe into fmuv4 rc.board.

ROMFS/px4fmu_common/init.d/airframes/4250_teal

@@ -16,7 +16,6 @@
 # @output MAIN4 motor 4
 #
 # @maintainer Jacob Dahl <jacob.dahl@tealdrones.com>
-# @maintainer Alex Klimaj <alex.klimaj@tealdrones.com>
 #
 
 echo "Executing 4250_teal script."
@@ -25,10 +24,12 @@ sh /etc/init.d/rc.mc_defaults
 
 set MIXER quad_x
 set PWM_OUT 1234
-set MIXER_AUX none
 
 if [ $AUTOCNF = yes ]
 then
+	# First thing, reset all params to default... EXCEPT THIS LIST
+	param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO* CAL_MAG* SENS_BOARD* EKF2_MAGBIAS*
+
 	# battery
 	param set BAT_CAPACITY 2750
 	param set BAT_CRIT_THR 0.15
@@ -42,17 +43,6 @@ then
 	param set BAT_V_EMPTY 3.65
 	param set BAT_V_OFFS_CURR -0.0045
 
-	# primary accel
-	param set CAL_ACC_PRIME 1442826 #mpu6500
-	#param set CAL_ACC_PRIME 1445386 #mpu9250
-
-	# primary gyro
-	param set CAL_GYRO_PRIME 2360330 #mpu6500
-	#param set CAL_GYRO_PRIME 2362890 #mpu9250
-
-	#primary Mag
-	#param set CAL_MAG_PRIME 265738 #mpu9250
-
 	# sensor calibration
 	param set CAL_MAG0_ROT 0
 	param set CAL_MAG_SIDES 63
@@ -65,36 +55,47 @@ then
 	param set CBRK_USB_CHK 197848
 
 	# commander
-	param set COM_DISARM_LAND 1
+	param set COM_DISARM_LAND 0.1
+	# Return mode at critically low level, Land mode at current position if reaching dangerously low levels.
 	param set COM_LOW_BAT_ACT 3
-	param set COM_LOSS_LTR_T 20
 
 	# ekf2
 	param set EKF2_AID_MASK 1
+	param set EKF2_MAG_TYPE 1
 	param set EKF2_GPS_CHECK 511
 	param set EKF2_GPS_POS_X -0.04
 	param set EKF2_IMU_POS_X -0.06
-	param set EKF2_MIN_RNG 0.07
 	param set EKF2_PCOEF_XN 0.1
 	param set EKF2_PCOEF_XP -0.5
 	param set EKF2_RNG_AID 1
 	param set EKF2_RNG_A_VMAX 20
 	param set EKF2_RNG_NOISE 0.2
+	param set EKF2_MIN_RNG 0.07
 
 	# gps
 	param set GPS_UBX_DYNMODEL 7
 
 	# geofence
+	# Geofence violation action -- Warning.
 	param set GF_ACTION 1
 
 	# land detector
-	param set LNDMC_THR_RANGE 0.50
 	param set LNDMC_XY_VEL_MAX 1
+	# This is set high because we have lots of vibrations while in contact with ground.
 	param set LNDMC_ROT_MAX 50
 
+	# serial comms
+	param set SER_TEL1_BAUD 921600
+	param set SER_TEL2_BAUD 921600
+
 	# mavlink stream configuration
+	# TEL1 ttyS1 -- disabled. TX1 FTDI UART has issues.
+	param set MAV_0_CONFIG 0
+	param set MAV_0_RATE 800000
+
+	# TEL2 ttyS2 -- Primary MAVLINK stream to companion computer.
 	param set MAV_1_CONFIG 102
-	param set MAV_1_RATE 20000
+	param set MAV_1_RATE 800000
 
 	# mc_att_control
 	param set MC_ACRO_P_MAX 360
@@ -107,6 +108,7 @@ then
 	param set MC_ROLLRATE_D 0.0012
 	param set MC_ROLLRATE_MAX 180
 
+	param set MC_PITCH_P 6.5
 	param set MC_PITCHRATE_P 0.06
 	param set MC_PITCHRATE_I 0.2
 	param set MC_PITCHRATE_D 0.0012
@@ -118,48 +120,62 @@ then
 	param set MC_YAWRATE_D 0
 	param set MC_YAWRATE_MAX 180
 
+	# Set to reduce voltage transients as seen by battery management system.
 	param set MOT_SLEW_MAX 0.15
 
-	# mc_pos_control
-	param set MPC_ACC_DOWN_MAX 10
-	param set MPC_ACC_HOR 10
-	param set MPC_ACC_HOR_MAX 15
-	param set MPC_ACC_UP_MAX 10
-	param set MPC_JERK_MAX 5
+	#### CONTROLLER ###
 	param set MPC_LAND_ALT1 8
 	param set MPC_LAND_ALT2 5
+	param set MPC_TKO_RAMP_T 0.75
+	param set MPC_TKO_SPEED 0.75
+	param set MPC_TILTMAX_LND 18
+	param set MPC_TILTMAX_AIR 45
+	param set MPC_MAN_TILT_MAX 45
+
 	param set MPC_MANTHR_MAX 0.85
 	param set MPC_MANTHR_MIN 0.15
-	param set MPC_MAN_TILT_MAX 45
-	param set MPC_MAN_Y_MAX 200
+	# Full throttle can trip over current protection on BMS.
 	param set MPC_THR_MAX 0.85
 	param set MPC_THR_MIN 0.15
-	param set MPC_TILTMAX_AIR 45
-	param set MPC_TKO_RAMP_T 0.75
-	param set MPC_TKO_SPEED 0.75
+
 	param set MPC_VEL_MANUAL 26.5
+	# RTL speed, it was too fast and scaring people.
 	param set MPC_XY_CRUISE 15
+
+	param set MPC_MAN_Y_MAX 200
+
+	param set MPC_JERK_MAX 5
+	param set MPC_ACC_UP_MAX 10
+	param set MPC_ACC_DOWN_MAX 10
+	param set MPC_ACC_HOR 10
+	param set MPC_ACC_HOR_MAX 15
+
 	param set MPC_XY_P 1.15
 	param set MPC_XY_VEL_P 0.14
 	param set MPC_XY_VEL_I 0.014
 	param set MPC_XY_VEL_D 0.014
 	param set MPC_XY_VEL_MAX 26.5
-	param set MPC_Z_P 0.8
-	param set MPC_TILTMAX_LND 18
 
+	param set MPC_Z_P 0.85
+	param set MPC_Z_VEL_P 0.25
+	param set MPC_Z_VEL_I 0.085
 	param set MPC_Z_VEL_D 0.02
+	# Documentation says limit is 8.0, but does not seem to be enforced in code.
+	param set MPC_Z_VEL_MAX_UP 20
 	param set MPC_Z_VEL_MAX_DN 2.5
-	param set MPC_Z_VEL_MAX_UP 6
+	#### CONTROLLER ###
 
 	# navigator
 	param set NAV_ACC_RAD 2.5
+	# RC loss failsafe behavior -- hold mode.
 	param set NAV_RCL_ACT 1
 
 	# pwm control
 	param set PWM_DISARMED 900
 	param set PWM_MAX 1850
 	param set PWM_MIN 1075
-	param set PWM_RATE 400
+	# Oneshot125
+	param set PWM_RATE 0
 
 	# rtl
 	param set RTL_DESCEND_ALT 5
@@ -167,11 +183,26 @@ then
 	param set RTL_MIN_DIST 7.5
 	param set RTL_RETURN_ALT 25
 
-	# sensors
-	param set SENS_EN_PGA460 1
-	param set SENS_EN_THERMAL 1
+	# calibration
+	param set CAL_ACC0_EN 1
+	param set CAL_ACC1_EN 0
+	#mpu6500
+	param set CAL_ACC_PRIME 1442826
 
-	# serial comms
-	param set SER_TEL2_BAUD 921600
+	param set CAL_GYRO0_EN 1
+	param set CAL_GYRO1_EN 0
+	#mpu6500
+	param set CAL_GYRO_PRIME 2360330
+
+	param set CAL_MAG0_EN 1
+	param set CAL_MAG1_EN 0
+	param set CAL_MAG_PRIME 265738
 
 fi
+
+# Do not start frsky_telemetry on port ttyS6 by default, PGA460 lives there. 500 is in arbitrary unused number.
+param set TEL_FRSKY_CONFIG 500
+# We want to make sure these always start
+param set SENS_EN_PGA460 1
+param set SENS_EN_THERMAL 1
+param set SENS_EN_BATT 1

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -55,6 +55,11 @@ then
 	fi
 fi
 
+if param compare SENS_EN_BATT 1
+then
+	batt_smbus start -X
+fi
+
 # Sensors on the PWM interface bank
 if param compare SENS_EN_LL40LS 1
 then

boards/px4/fmu-v4/init/rc.board

@@ -44,6 +44,13 @@ bmp280 -I start
 # expansion i2c used for BMM150 rotated by 90deg
 bmm150 -R 2 start
 
+# For Teal One airframe
+if param compare SYS_AUTOSTART 4250
+then
+	mpu9250 -s -R 14 start
+	mpu9250 -t -R 14 start
+fi
+
 # hmc5883 internal SPI bus is rotated 90 deg yaw
 if ! hmc5883 -C -T -S -R 2 start
 then

src/drivers/batt_smbus/parameters.c

@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * SMBUS Smart battery driver (BQ40Z50)
+ *
+ * @reboot_required true
+ *
+ * @boolean
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_EN_BATT, 0);