[gentlenav] r3673 committed - MatrixPilot_trunk: merge of helical turns support as recently tested i...

[gent...@googlecode.com]

Revision: 3673
Author: robert.d...@gmail.com
Date: Sat Aug 1 04:50:37 2015 UTC
Log: MatrixPilot_trunk: merge of helical turns support as recently
tested in branch MatrixPilot_helicalTurns.
https://code.google.com/p/gentlenav/source/detail?r=3673

Modified:
/trunk/Config/Cessna/options.h
/trunk/Config/CloudsFly/options.h
/trunk/Config/magnetometerOptions.h
/trunk/Config/options.h
/trunk/MatrixPilot/MAVLink.c
/trunk/MatrixPilot/MAVUDBExtra.c
/trunk/MatrixPilot/Readme.txt
/trunk/MatrixPilot/config.c
/trunk/MatrixPilot/config.h
/trunk/MatrixPilot/config_tests.c
/trunk/MatrixPilot/console.c
/trunk/MatrixPilot/example-options-files/options.EasyStar-Bill.h
/trunk/MatrixPilot/flightplan-waypoints.c
/trunk/MatrixPilot/gain_variables.h
/trunk/MatrixPilot/helicalTurnCntrl.c
/trunk/MatrixPilot/helicalTurnCntrl.h
/trunk/MatrixPilot/navigate.c
/trunk/MatrixPilot/navigate.h
/trunk/MatrixPilot/nv_memory_table.c
/trunk/MatrixPilot/parameter_table.c
/trunk/MatrixPilot/parameter_table.h
/trunk/MatrixPilot/parameter_table2.c
/trunk/MatrixPilot/parameter_table_init.c
/trunk/MatrixPilot/pitchCntrl.c
/trunk/MatrixPilot/ring_buffer.c
/trunk/MatrixPilot/rollCntrl.c
/trunk/MatrixPilot/servoMix.c
/trunk/MatrixPilot/servoPrepare.h
/trunk/MatrixPilot/telemetry.c
/trunk/MatrixPilot/telemetry.h
/trunk/MatrixPilot/yawCntrl.c
/trunk/Tools/MAVLink/mavlink/message_definitions/v1.0/common.xml
/trunk/Tools/MatrixPilot-SIL/SIL-udb.c
/trunk/Tools/MatrixPilot-SIL/module.mk
/trunk/Tools/flight_analyzer/check_telemetry_type.py
/trunk/Tools/flight_analyzer/matrixpilot_lib.py
/trunk/Tools/makefiles/device-UDB5.mk
/trunk/Tools/pyparam/ParameterDatabase.xml
/trunk/libDCM/estAltitude.c
/trunk/libDCM/estWind.c
/trunk/libDCM/estWind.h
/trunk/libDCM/gpsParseCommon.c
/trunk/libDCM/gpsParseCommon.h
/trunk/libDCM/gpsParseUBX.c
/trunk/libDCM/libDCM.c
/trunk/libDCM/libDCM.h
/trunk/libDCM/mag_drift.h
/trunk/libDCM/rmat.c
/trunk/libDCM/rmat.h
/trunk/libSTM/libSTM.c
/trunk/libUDB/ADchannel.c
/trunk/libUDB/fixDeps.vbs
/trunk/libUDB/libUDB.h
/trunk/libUDB/magnetometer.c

=======================================
--- /trunk/Config/Cessna/options.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/Config/Cessna/options.h Sat Aug 1 04:50:37 2015 UTC
@@ -80,7 +80,7 @@
#define YAW_STABILIZATION_AILERON 1

// Aileron and Rudder Navigation
-// Set either of these to 0 to disable use of that control surface for
navigation.
+// Set either of these to 1 to enable helical turn control for navigation.
#define AILERON_NAVIGATION 1
#define RUDDER_NAVIGATION 1

@@ -454,8 +454,9 @@
// All gains should be positive real numbers.
// Proportional gains should be less than 4.0.
// Rate gains should be less than 0.8.
-// Proportional gains include ROLLKP, YAWKP_AILERON, AILERON_BOOST,
PITCHGAIN,
-// RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST, YAWKP_RUDDER,
ROLLKP_RUDDER,
+// With the new helical turn control, rate gains are not even needed, try
setting them all to zero.
+// Proportional gains include ROLLKP, YAWKP_AILERON, PITCHGAIN,
+// ELEVATOR_BOOST, YAWKP_RUDDER, ROLLKP_RUDDER,
// MANUAL_AILERON_RUDDER_MIX, RUDDER_BOOST, HOVER_ROLLKP, HOVER_PITCHGAIN,
HOVER_YAWKP
// Rate gains include ROLLKD, YAWKD_AILERON, PITCHKD, YAWKD_RUDDER,
ROLLKD_RUDDER,
// HOVER_ROLLKD, HOVER_PITCHKD, HOVER_YAWKD
@@ -464,41 +465,69 @@
// set it to 1.0 if you want full servo throw, otherwise set it to the
portion that you want
#define SERVOSAT 1.0

+// FEED_FORWARD is a feed forward gain for deflecting control surfaces for
turn rate.
+// The KP gains for each axis are multiplied by FEED_FORWARD to determine
+// the feed forward gain for that axis.
+// For each axis, a deflection term is added equal to the feed forward
gain for that axis
+// times projection of the desired earth vertical rotation rate onto that
axis
+#define FEED_FORWARD 1.0
+
+// TURN_RATE_NAV and TURN_RATE_FBW set the gains of the helical turn
control for
+// waypoint navigation mode and fly by wire mode respectively.
+// They are specified in terms of the maximum desired turning rate in
degrees per second in each mode.
+// The largest possible value is 240 degrees per second, anything larger
will be clipped to 240.
+#define TURN_RATE_NAV 15.0
+#define TURN_RATE_FBW 30.0
+
// Aileron/Roll Control Gains
// ROLLKP is the proportional gain, approximately 0.25
// ROLLKD is the derivative (gyro) gain, approximately 0.125
-// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error
-// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation
-// AILERON_BOOST is the additional gain multiplier for the manually
commanded aileron deflection
+// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error.
+// use it only if there is no rudder.
+// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation.
+// use it only if there is no rudder.
#define ROLLKP 0.08
#define ROLLKD 0.04
#define YAWKP_AILERON 0.04
#define YAWKD_AILERON 0.04
-#define AILERON_BOOST 0.8

// Elevator/Pitch Control Gains
// PITCHGAIN is the pitch stabilization gain, typically around 0.125
// PITCHKD feedback gain for pitch damping, around 0.0625
-// RUDDER_ELEV_MIX is the degree of elevator adjustment for rudder and
banking
-// AILERON_ELEV_MIX is the degree of elevator adjustment for aileron
// ELEVATOR_BOOST is the additional gain multiplier for the manually
commanded elevator deflection
#define PITCHGAIN 0.2
#define PITCHKD 0.15
-#define RUDDER_ELEV_MIX 0.2
-#define ROLL_ELEV_MIX 0.35
#define ELEVATOR_BOOST 0.8

-// Neutral pitch angle of the plane (in degrees) when flying inverted
-// Use this to add extra "up" elevator while the plane is inverted, to
avoid losing altitude.
-#define INVERTED_NEUTRAL_PITCH 12.0
+// Parameters below are used in the computation of angle of attack and
pitch trim.
+// ( INVERTED_NEUTRAL_PITCH is no longer used and should not be used.)
+// If these parameters are not defined, angle of attack and pitch trim
will be set to zero.
+// CRUISE_SPEED The nominal speed in meters per
second at which the parameters are defined.
+// ANGLE_OF_ATTACK_NORMAL Angle of attack in degrees in the
body frame for normal straight and level flight at cruise speed.
+// ANGLE_OF_ATTACK_INVERTED Angle of attack in degrees in the
body frame for inverted straight and level flight at cruise speed.
+// Note: ANGLE_OF_ATTACK_INVERTED is usually negative, with typical values
in the -5 to -10 degree range.
+// ELEVATOR_TRIM_NORMAL Elevator trim in fractional servo
units (-1.0 to 1.0 ) for normal straight and level flight at cruise speed.
+// ELEVATOR_TRIM_INVERTED Elevator trim in fractional servo
units (-1.0 to 1.0 ) for inverted straight and level flight at cruise speed.
+// Note: ELEVATOR_TRIM_INVERTED is usually negative, with typical values
in the -0.5 to -1.0 range.
+
+// The following are the values for HILSIM EasyStar2:
+//#define CRUISE_SPEED ( 12.0 )
+#define CRUISE_SPEED ( 36.0 ) // RobD: modified
for XPlane-Cessna
+#define ANGLE_OF_ATTACK_NORMAL ( -0.8 )
+#define ANGLE_OF_ATTACK_INVERTED ( -7.2 )
+#define ELEVATOR_TRIM_NORMAL ( -0.03 )
+#define ELEVATOR_TRIM_INVERTED ( -0.67 )

// Rudder/Yaw Control Gains
-// YAWKP_RUDDER is the proportional feedback gain for rudder navigation
-// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation
-// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle
-// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle
+// YAWKP_RUDDER is the proportional feedback gain for rudder control of
yaw orientation.
+// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation.
+// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle,
+// use it only if there are no ailerons.
+// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle,
+// use it only if there are no ailerons.
// MANUAL_AILERON_RUDDER_MIX is the fraction of manual aileron control to
mix into the rudder when
// in stabilized or waypoint mode. This mainly helps aileron-initiated
turning while in stabilized.
+// MANUAL_AILERON_RUDDER_MIX is no longer needed with the new controls, it
should be set to zero.
// RUDDER_BOOST is the additional gain multiplier for the manually
commanded rudder deflection
#define YAWKP_RUDDER 0.06
#define YAWKD_RUDDER 0.03
@@ -508,6 +537,7 @@
#define RUDDER_BOOST 0.8

// Gains for Hovering
+// These are still here from the previous version of the controls, because
the new controls have not yet been set up for hovering.
// Gains are named based on plane's frame of reference (roll means
ailerons)
// HOVER_ROLLKP is the roll-proportional feedback gain applied to the
ailerons while navigating a hover
// HOVER_ROLLKD is the roll gyro feedback gain applied to ailerons while
stabilizing a hover
=======================================
--- /trunk/Config/CloudsFly/options.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/Config/CloudsFly/options.h Sat Aug 1 04:50:37 2015 UTC
@@ -83,7 +83,7 @@
#define YAW_STABILIZATION_AILERON 1

// Aileron and Rudder Navigation
-// Set either of these to 0 to disable use of that control surface for
navigation.
+// Set either of these to 1 to enable helical turn control for navigation.
#define AILERON_NAVIGATION 1
#define RUDDER_NAVIGATION 1

@@ -456,8 +456,9 @@
// All gains should be positive real numbers.
// Proportional gains should be less than 4.0.
// Rate gains should be less than 0.8.
-// Proportional gains include ROLLKP, YAWKP_AILERON, AILERON_BOOST,
PITCHGAIN,
-// RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST, YAWKP_RUDDER,
ROLLKP_RUDDER,
+// With the new helical turn control, rate gains are not even needed, try
setting them all to zero.
+// Proportional gains include ROLLKP, YAWKP_AILERON, PITCHGAIN,
+// ELEVATOR_BOOST, YAWKP_RUDDER, ROLLKP_RUDDER,
// MANUAL_AILERON_RUDDER_MIX, RUDDER_BOOST, HOVER_ROLLKP, HOVER_PITCHGAIN,
HOVER_YAWKP
// Rate gains include ROLLKD, YAWKD_AILERON, PITCHKD, YAWKD_RUDDER,
ROLLKD_RUDDER,
// HOVER_ROLLKD, HOVER_PITCHKD, HOVER_YAWKD
@@ -466,52 +467,78 @@
// set it to 1.0 if you want full servo throw, otherwise set it to the
portion that you want
#define SERVOSAT 1.0

+// FEED_FORWARD is a feed forward gain for deflecting control surfaces for
turn rate.
+// The KP gains for each axis are multiplied by FEED_FORWARD to determine
+// the feed forward gain for that axis.
+// For each axis, a deflection term is added equal to the feed forward
gain for that axis
+// times projection of the desired earth vertical rotation rate onto that
axis
+#define FEED_FORWARD 1.0
+
+// TURN_RATE_NAV and TURN_RATE_FBW set the gains of the helical turn
control for
+// waypoint navigation mode and fly by wire mode respectively.
+// They are specified in terms of the maximum desired turning rate in
degrees per second in each mode.
+// The largest possible value is 240 degrees per second, anything larger
will be clipped to 240.
+#define TURN_RATE_NAV 30.0
+#define TURN_RATE_FBW 60.0
+
// Aileron/Roll Control Gains
// ROLLKP is the proportional gain, approximately 0.25
// ROLLKD is the derivative (gyro) gain, approximately 0.125
-// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error
-// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation
-// AILERON_BOOST is the additional gain multiplier for the manually
commanded aileron deflection
+// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error.
+// use it only if there is no rudder.
+// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation.
+// use it only if there is no rudder.
#define ROLLKP 0.20
#define ROLLKD 0.05
-#define YAWKP_AILERON 0.10
-#define YAWKD_AILERON 0.05
-#define AILERON_BOOST 1.00
+#define YAWKP_AILERON 0.00
+#define YAWKD_AILERON 0.00

// Elevator/Pitch Control Gains
// PITCHGAIN is the pitch stabilization gain, typically around 0.125
// PITCHKD feedback gain for pitch damping, around 0.0625
-// RUDDER_ELEV_MIX is the degree of elevator adjustment for rudder and
banking
-// AILERON_ELEV_MIX is the degree of elevator adjustment for aileron
// ELEVATOR_BOOST is the additional gain multiplier for the manually
commanded elevator deflection
-//#define PITCHGAIN 0.10
-#define PITCHGAIN 3.90
-#define PITCHKD 0.04
-#define RUDDER_ELEV_MIX 0.20
-#define ROLL_ELEV_MIX 0.05
+#define PITCHGAIN 0.30
+#define PITCHKD 0.00
#define ELEVATOR_BOOST 0.50

-// Neutral pitch angle of the plane (in degrees) when flying inverted
-// Use this to add extra "up" elevator while the plane is inverted, to
avoid losing altitude.
-#define INVERTED_NEUTRAL_PITCH 8.0
+// Parameters below are used in the computation of angle of attack and
pitch trim.
+// ( INVERTED_NEUTRAL_PITCH is no longer used and should not be used.)
+// If these parameters are not defined, angle of attack and pitch trim
will be set to zero.
+// CRUISE_SPEED The nominal speed in meters per
second at which the parameters are defined.
+// ANGLE_OF_ATTACK_NORMAL Angle of attack in degrees in the
body frame for normal straight and level flight at cruise speed.
+// ANGLE_OF_ATTACK_INVERTED Angle of attack in degrees in the
body frame for inverted straight and level flight at cruise speed.
+// Note: ANGLE_OF_ATTACK_INVERTED is usually negative, with typical values
in the -5 to -10 degree range.
+// ELEVATOR_TRIM_NORMAL Elevator trim in fractional servo
units (-1.0 to 1.0 ) for normal straight and level flight at cruise speed.
+// ELEVATOR_TRIM_INVERTED Elevator trim in fractional servo
units (-1.0 to 1.0 ) for inverted straight and level flight at cruise speed.
+// Note: ELEVATOR_TRIM_INVERTED is usually negative, with typical values
in the -0.5 to -1.0 range.
+
+// The following are the values for HILSIM EasyStar2:
+#define CRUISE_SPEED ( 12.0 )
+#define ANGLE_OF_ATTACK_NORMAL ( -0.8 )
+#define ANGLE_OF_ATTACK_INVERTED ( -7.2 )
+#define ELEVATOR_TRIM_NORMAL ( -0.03 )
+#define ELEVATOR_TRIM_INVERTED ( -0.67 )

// Rudder/Yaw Control Gains
-// YAWKP_RUDDER is the proportional feedback gain for rudder navigation
-// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation
-// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle
-// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle
+// YAWKP_RUDDER is the proportional feedback gain for rudder control of
yaw orientation.
+// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation.
+// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle,
+// use it only if there are no ailerons.
+// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle,
+// use it only if there are no ailerons.
// MANUAL_AILERON_RUDDER_MIX is the fraction of manual aileron control to
mix into the rudder when
// in stabilized or waypoint mode. This mainly helps aileron-initiated
turning while in stabilized.
+// MANUAL_AILERON_RUDDER_MIX is no longer needed with the new controls, it
should be set to zero.
// RUDDER_BOOST is the additional gain multiplier for the manually
commanded rudder deflection
-//#define YAWKP_RUDDER 0.05
-#define YAWKP_RUDDER 3.90
-#define YAWKD_RUDDER 0.05
-#define ROLLKP_RUDDER 0.06
-#define ROLLKD_RUDDER 0.05
+#define YAWKP_RUDDER 0.30
+#define YAWKD_RUDDER 0.00
+#define ROLLKP_RUDDER 0.00
+#define ROLLKD_RUDDER 0.00
#define MANUAL_AILERON_RUDDER_MIX 0.00
-#define RUDDER_BOOST 1.00
+#define RUDDER_BOOST 0.50

// Gains for Hovering
+// These are still here from the previous version of the controls, because
the new controls have not yet been set up for hovering.
// Gains are named based on plane's frame of reference (roll means
ailerons)
// HOVER_ROLLKP is the roll-proportional feedback gain applied to the
ailerons while navigating a hover
// HOVER_ROLLKD is the roll gyro feedback gain applied to ailerons while
stabilizing a hover
=======================================
--- /trunk/Config/magnetometerOptions.h Sat Sep 27 09:32:43 2014 UTC
+++ /trunk/Config/magnetometerOptions.h Sat Aug 1 04:50:37 2015 UTC
@@ -42,7 +42,7 @@
#define MAGNETICDECLINATION 0

// Set to 0 for fixed declination angle or 1 for variable declination angle
-#define DECLINATIONANGLE_VARIABLE 0
+#define DECLINATIONANGLE_VARIABLE 1


// #define LED_RED_MAG_CHECK 1 if you want the RED LED to indicate the
magnetometer is not working.
@@ -271,5 +271,3 @@

// The following line computes an internal parameter, do not change it.
#define DECLINATIONANGLE ((int16_t)(MAGNETICDECLINATION *(32767.0 /
180.0)))
-
-
=======================================
--- /trunk/Config/options.h Thu May 7 21:42:36 2015 UTC
+++ /trunk/Config/options.h Sat Aug 1 04:50:37 2015 UTC
@@ -2,7 +2,7 @@
//
// http://code.google.com/p/gentlenav/
//
-// Copyright 2009-2014 MatrixPilot Team
+// Copyright 2009-2015 MatrixPilot Team
// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
//
// MatrixPilot is free software: you can redistribute it and/or modify
@@ -77,10 +77,10 @@
#define ROLL_STABILIZATION_RUDDER 0
#define PITCH_STABILIZATION 1
#define YAW_STABILIZATION_RUDDER 1
-#define YAW_STABILIZATION_AILERON 1
+#define YAW_STABILIZATION_AILERON 0

// Aileron and Rudder Navigation
-// Set either of these to 0 to disable use of that control surface for
navigation.
+// Set either of these to 1 to enable helical turn control for navigation.
#define AILERON_NAVIGATION 1
#define RUDDER_NAVIGATION 1

@@ -351,13 +351,6 @@
//#define SERIAL_BAUDRATE 19200


-////////////////////////////////////////////////////////////////////////////////
-
-// MAVLink requires an aircraft Identifier (I.D) as it is designed to
control multiple aircraft
-// Each aircraft in the sky will need a unique I.D. in the range from 0-255
-//#define MAVLINK_SYSID 1 // now defined in
mavlink_options.h
-
-
// NUM_ANALOG_INPUTS:
// For UDB4 boards: Set to 0-4. Analog pins are AN15 - AN18.
//#define NUM_ANALOG_INPUTS 0 // moved to board specific
config files
@@ -381,7 +374,7 @@
// UDB, in order to see the RC signal strength on your OSD. Just plug
RSSI and ground
// from your Receiver to Input2's signal and ground on your UDB. If you
use this feature,
// you'll also need to set up the RSSI_MIN_SIGNAL_VOLTAGE and
RSSI_MAX_SIGNAL_VOLTAGE
-// to match your Receiver's RSSI format. Note that some receivers use a
higher voltage to
+// to match your Receiver's RSSI format. Note that some receivers use a
higher voltage to
// represent a lower signal strength, so you may need to set MIN higher
than MAX.

#define ANALOG_CURRENT_INPUT_CHANNEL CHANNEL_UNUSED
@@ -404,7 +397,7 @@
// Designed for use with the following device:-
// http://www.maxbotix.com/Ultrasonic_Sensors/MB1230.htm
// Can be used on INPUT 8 of the UDB4/5 if that is not used for a channel
input.
-// Will return distance to ground in meters and compensate for roll
subject to
+// Will return distance to ground in meters and compensate for roll
subject to
// receiving a returned sonar signal.
// This option is designed to be used with Logo Flight Planning.
// Logo allows the user to Interrupt a Landing and flare, or Go Around,
@@ -454,8 +447,9 @@
// All gains should be positive real numbers.
// Proportional gains should be less than 4.0.
// Rate gains should be less than 0.8.
-// Proportional gains include ROLLKP, YAWKP_AILERON, AILERON_BOOST,
PITCHGAIN,
-// RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST, YAWKP_RUDDER,
ROLLKP_RUDDER,
+// With the new helical turn control, rate gains are not even needed, try
setting them all to zero.
+// Proportional gains include ROLLKP, YAWKP_AILERON, PITCHGAIN,
+// ELEVATOR_BOOST, YAWKP_RUDDER, ROLLKP_RUDDER,
// MANUAL_AILERON_RUDDER_MIX, RUDDER_BOOST, HOVER_ROLLKP, HOVER_PITCHGAIN,
HOVER_YAWKP
// Rate gains include ROLLKD, YAWKD_AILERON, PITCHKD, YAWKD_RUDDER,
ROLLKD_RUDDER,
// HOVER_ROLLKD, HOVER_PITCHKD, HOVER_YAWKD
@@ -464,50 +458,78 @@
// set it to 1.0 if you want full servo throw, otherwise set it to the
portion that you want
#define SERVOSAT 1.0

+// FEED_FORWARD is a feed forward gain for deflecting control surfaces for
turn rate.
+// The KP gains for each axis are multiplied by FEED_FORWARD to determine
+// the feed forward gain for that axis.
+// For each axis, a deflection term is added equal to the feed forward
gain for that axis
+// times projection of the desired earth vertical rotation rate onto that
axis
+#define FEED_FORWARD 1.0
+
+// TURN_RATE_NAV and TURN_RATE_FBW set the gains of the helical turn
control for
+// waypoint navigation mode and fly by wire mode respectively.
+// They are specified in terms of the maximum desired turning rate in
degrees per second in each mode.
+// The largest possible value is 240 degrees per second, anything larger
will be clipped to 240.
+#define TURN_RATE_NAV 30.0
+#define TURN_RATE_FBW 60.0
+
// Aileron/Roll Control Gains
// ROLLKP is the proportional gain, approximately 0.25
// ROLLKD is the derivative (gyro) gain, approximately 0.125
-// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error
-// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation
-// AILERON_BOOST is the additional gain multiplier for the manually
commanded aileron deflection
+// YAWKP_AILERON is the proportional feedback gain for ailerons in
response to yaw error.
+// use it only if there is no rudder.
+// YAWKD_AILERON is the derivative feedback gain for ailerons in response
to yaw rotation.
+// use it only if there is no rudder.
#define ROLLKP 0.20
#define ROLLKD 0.05
-#define YAWKP_AILERON 0.10
-#define YAWKD_AILERON 0.05
-#define AILERON_BOOST 1.00
+#define YAWKP_AILERON 0.00
+#define YAWKD_AILERON 0.00

// Elevator/Pitch Control Gains
// PITCHGAIN is the pitch stabilization gain, typically around 0.125
// PITCHKD feedback gain for pitch damping, around 0.0625
-// RUDDER_ELEV_MIX is the degree of elevator adjustment for rudder and
banking
-// AILERON_ELEV_MIX is the degree of elevator adjustment for aileron
// ELEVATOR_BOOST is the additional gain multiplier for the manually
commanded elevator deflection
-#define PITCHGAIN 0.10
-#define PITCHKD 0.04
-#define RUDDER_ELEV_MIX 0.20
-#define ROLL_ELEV_MIX 0.05
+#define PITCHGAIN 0.30
+#define PITCHKD 0.00
#define ELEVATOR_BOOST 0.50

-// Neutral pitch angle of the plane (in degrees) when flying inverted
-// Use this to add extra "up" elevator while the plane is inverted, to
avoid losing altitude.
-#define INVERTED_NEUTRAL_PITCH 8.0
+// Parameters below are used in the computation of angle of attack and
pitch trim.
+// ( INVERTED_NEUTRAL_PITCH is no longer used and should not be used.) --
Note (RobD) yes it is?
+// If these parameters are not defined, angle of attack and pitch trim
will be set to zero.
+// CRUISE_SPEED The nominal speed in meters per
second at which the parameters are defined.
+// ANGLE_OF_ATTACK_NORMAL Angle of attack in degrees in the
body frame for normal straight and level flight at cruise speed.
+// ANGLE_OF_ATTACK_INVERTED Angle of attack in degrees in the
body frame for inverted straight and level flight at cruise speed.
+// Note: ANGLE_OF_ATTACK_INVERTED is usually negative, with typical values
in the -5 to -10 degree range.
+// ELEVATOR_TRIM_NORMAL Elevator trim in fractional servo
units (-1.0 to 1.0 ) for normal straight and level flight at cruise speed.
+// ELEVATOR_TRIM_INVERTED Elevator trim in fractional servo
units (-1.0 to 1.0 ) for inverted straight and level flight at cruise speed.
+// Note: ELEVATOR_TRIM_INVERTED is usually negative, with typical values
in the -0.5 to -1.0 range.
+
+// The following are the values for HILSIM EasyStar2:
+#define CRUISE_SPEED ( 12.0 )
+#define ANGLE_OF_ATTACK_NORMAL ( -0.8 )
+#define ANGLE_OF_ATTACK_INVERTED ( -7.2 )
+#define ELEVATOR_TRIM_NORMAL ( -0.03 )
+#define ELEVATOR_TRIM_INVERTED ( -0.67 )

// Rudder/Yaw Control Gains
-// YAWKP_RUDDER is the proportional feedback gain for rudder navigation
-// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation
-// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle
-// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle
+// YAWKP_RUDDER is the proportional feedback gain for rudder control of
yaw orientation.
+// YAWKD_RUDDER is the yaw gyro feedback gain for the rudder in reponse to
yaw rotation.
+// ROLLKP_RUDDER is the feedback gain for the rudder in response to the
current roll angle,
+// use it only if there are no ailerons.
+// ROLLKD_RUDDER is the feedback gain for the rudder in response to the
rate of change roll angle,
+// use it only if there are no ailerons.
// MANUAL_AILERON_RUDDER_MIX is the fraction of manual aileron control to
mix into the rudder when
// in stabilized or waypoint mode. This mainly helps aileron-initiated
turning while in stabilized.
+// MANUAL_AILERON_RUDDER_MIX is no longer needed with the new controls, it
should be set to zero.
// RUDDER_BOOST is the additional gain multiplier for the manually
commanded rudder deflection
-#define YAWKP_RUDDER 0.05
-#define YAWKD_RUDDER 0.05
-#define ROLLKP_RUDDER 0.06
-#define ROLLKD_RUDDER 0.05
+#define YAWKP_RUDDER 0.30
+#define YAWKD_RUDDER 0.00
+#define ROLLKP_RUDDER 0.00
+#define ROLLKD_RUDDER 0.00
#define MANUAL_AILERON_RUDDER_MIX 0.00
-#define RUDDER_BOOST 1.00
+#define RUDDER_BOOST 0.50

// Gains for Hovering
+// These are still here from the previous version of the controls, because
the new controls have not yet been set up for hovering.
// Gains are named based on plane's frame of reference (roll means
ailerons)
// HOVER_ROLLKP is the roll-proportional feedback gain applied to the
ailerons while navigating a hover
// HOVER_ROLLKD is the roll gyro feedback gain applied to ailerons while
stabilizing a hover
@@ -560,7 +582,7 @@

// Setup and configuration of camera targetting at installation of camera
servos:-
// To save cpu cycles, you will need to pre-compute the tangent of the
desired pitch of the camera
-// when in stabilized mode. This should be expressed in 2:14 format.
+// when in stabilized mode. This should be expressed in 2:14 format.
// Example: You require the camera to be pitched down by 15 degrees from
the horizon in stabilized mode.
// Paste the following line into a google search box (without the //)
// tan((( 15 /180 )* 3.1416 ))* 16384
@@ -571,17 +593,17 @@
#define CAM_YAW_IN_STABILIZED_MODE 0 // in degrees relative
to the plane's yaw axis. Example: 0

// All number should be integers
-#define CAM_PITCH_SERVO_THROW 95 // Camera lens
rotation at maximum PWM change (2000 to 4000), in degrees.
-#define CAM_PITCH_SERVO_MAX 85 // Max pitch up that
plane can tilt and keep camera level, in degrees.
-#define CAM_PITCH_SERVO_MIN -22 // Max pitch down that
plane can tilt and keep camera level, in degrees.
-#define CAM_PITCH_OFFSET_CENTRED 38 // Offset in degrees
of servo that results in a level camera.
+#define CAM_PITCH_SERVO_THROW 95 // Camera lens
rotation at maximum PWM change (2000 to 4000), in degrees.
+#define CAM_PITCH_SERVO_MAX 85 // Max pitch up that
plane can tilt and keep camera level, in degrees.
+#define CAM_PITCH_SERVO_MIN -22 // Max pitch down that
plane can tilt and keep camera level, in degrees.
+#define CAM_PITCH_OFFSET_CENTRED 38 // Offset in degrees
of servo that results in a level camera.
// Example: 30 would
mean that a centered pitch servo points the camera
// 30 degrees down
from horizontal when looking to the front of the plane.

-#define CAM_YAW_SERVO_THROW 350 // Camera yaw movement
for maximum yaw PWM change (2000 to 4000) in Degrees.
-#define CAM_YAW_SERVO_MAX 130 // Max positive yaw of
camera relative to front of plane in Degrees.
-#define CAM_YAW_SERVO_MIN -130 // Min reverse yaw of
camera relative to front of plane in Degrees.
-#define CAM_YAW_OFFSET_CENTRED 11 // Yaw offset in
degrees that results in camera pointing forward.
+#define CAM_YAW_SERVO_THROW 350 // Camera yaw movement
for maximum yaw PWM change (2000 to 4000) in Degrees.
+#define CAM_YAW_SERVO_MAX 130 // Max positive yaw of
camera relative to front of plane in Degrees.
+#define CAM_YAW_SERVO_MIN -130 // Min reverse yaw of
camera relative to front of plane in Degrees.
+#define CAM_YAW_OFFSET_CENTRED 11 // Yaw offset in
degrees that results in camera pointing forward.

// Camera test mode will move the yaw from + 90 degrees to + 90 degrees
every 5 seconds. (180 degree turn around)
// That will show whether the CAM_PITCH_SERVO_THROW value is set correctly
for your servo.
@@ -699,11 +721,6 @@
#define FLY_BY_DATALINK_ENABLED 0


-////////////////////////////////////////////////////////////////////////////////
-// Optionally enable the new power saving idle mode of the MCU during
mainloop
-//#define USE_MCU_IDLE 1 // moved to interrupt.h
-
-

////////////////////////////////////////////////////////////////////////////////
// Optionally enable experimental extended range navigation support
(merged from ballon launch branch)
//#define USE_EXTENDED_NAV
@@ -718,24 +735,27 @@
// #define TestGains // uncomment this line if you
want to test your gains without using GPS

// Set this to 1 to calculate and print out free stack space
+#ifndef RECORD_FREE_STACK_SPACE
#define RECORD_FREE_STACK_SPACE 0
+#endif



////////////////////////////////////////////////////////////////////////////////
// The UDB4/5 has two UART's, while the AUAV3 has four UART's.
-// Three MatrixPilot features are currently defined for using a UART.
+// Three MatrixPilot features are currently defined for using a UART.
// These being the GPS, Telemetry and a 'debug' console.
// Therefore UDB4/5 is one UART short, the AUAV3 has one UART extra.
//
// CONSOLE_UART specfies which UART is used for stdio support, aka the
console.
// Set CONSOLE_UART to 1, 2, 3 or 4 to enable the console on UART of that
number.
// Setting CONSOLE_UART to 0 disables console support.
-// On the UDB4/5, optionally specifying console support on UART 1 or 2
overrides
+// On the UDB4/5, optionally specifying console support on UART 1 or 2
overrides
// the default usage of that UART, being the GPS and Telemetry
respectively.
// CONSOLE_UART 3 and 4 options are only available with the AUAV3 board.
// Thus UDB4/5 options are 0, 1, or 2 AUAV3 options are 0, 3, or 4
// Set to 9 in order to use the USB for the console connection
#define CONSOLE_UART 0
+//#define CONSOLE_UART 6

// Define USE_DEBUG_IO to enable DPRINT macro to call printf(..)
//#define USE_DEBUG_IO
@@ -766,7 +786,7 @@
// On the AUAV3, the external UART connections are known as ports 1
through 4.
// The definitions below specifies which feature maps to an external port.
//
-// NOTE: on the AUAV3, do not confuse the CONSOLE_UART definition with the
+// NOTE: on the AUAV3, do not confuse the CONSOLE_UART definition with the
// external port assignment.
// Assign the console to an internal UART with CONSOLE_UART, map this
console to
// external port connection with DBG_PORT.
@@ -776,10 +796,10 @@

// Set this to 1 to enable filesystem support
#ifndef USE_FILESYS
-#define USE_FILESYS 1
+#define USE_FILESYS 0
#endif

-// Set this to 1 to enable logging telemetry to filesystem
+// Set this to 1 to enable logging telemetry to builtin filesystem
#ifndef USE_TELELOG
#define USE_TELELOG 0
#endif
=======================================
--- /trunk/MatrixPilot/MAVLink.c Sun Jun 7 16:01:09 2015 UTC
+++ /trunk/MatrixPilot/MAVLink.c Sat Aug 1 04:50:37 2015 UTC
@@ -1043,6 +1043,13 @@
{
MAVUDBExtraOutput_40hz();
}
+ // Send FORCE information
+ spread_transmission_load = 15;
+ if (mavlink_frequency_send(MAVLINK_RATE_FORCE, mavlink_counter_40hz +
spread_transmission_load))
+ {
+ mavlink_msg_force_send(MAVLINK_COMM_0, msec, aero_force[0],
aero_force[1], aero_force[2]);
+//static inline void mavlink_msg_force_send(mavlink_channel_t chan,
uint32_t time_boot_ms, int16_t aero_x, int16_t aero_y, int16_t aero_z)
+ }
MAVParamsOutput_40hz();
MAVMissionOutput_40hz();
MAVFlexiFunctionsOutput_40hz();
=======================================
--- /trunk/MatrixPilot/MAVUDBExtra.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/MAVUDBExtra.c Sat Aug 1 04:50:37 2015 UTC
@@ -83,11 +83,11 @@
break;
case 4:
mavlink_msg_serial_udb_extra_f5_send(MAVLINK_COMM_0,
gains.YawKPAileron, gains.YawKDAileron, gains.RollKP, gains.RollKD,
- settings._.YawStabilizationAileron, gains.AileronBoost);
+ settings._.YawStabilizationAileron, 0);
mavlink_sue_telemetry_counter--;
break;
case 3:
- mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_0, gains.Pitchgain,
gains.PitchKD, gains.RudderElevMix, gains.RollElevMix, gains.ElevatorBoost);
+ mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_0, gains.Pitchgain,
gains.PitchKD, 0, 0, gains.ElevatorBoost);
mavlink_sue_telemetry_counter--;
break;
case 2:
=======================================
--- /trunk/MatrixPilot/Readme.txt Fri Dec 6 12:11:18 2013 UTC
+++ /trunk/MatrixPilot/Readme.txt Sat Aug 1 04:50:37 2015 UTC
@@ -11,3 +11,12 @@
For more info about how to configure and use this autopilot firmware, go
to:

http://code.google.com/p/gentlenav/
+
+This version of MatrixPilot supports a fly-by-wire control mode using
helical turn controls.
+
+For information on how to set up and operate the controls, read the
following files that will be
+either this directory or in the directory one directory up from here:
+
+RecordingSensorOffsets.pdf
+HelicalTurnControlsSetup.pdf
+AngleofAttackandElevatorTrimModelsSetup.pdf
=======================================
--- /trunk/MatrixPilot/config.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/config.c Sat Aug 1 04:50:37 2015 UTC
@@ -31,6 +31,7 @@
struct gains_variables gains;
struct altit_variables altit;
struct hover_variables hover;
+struct turns_variables turns;

static const char* strConfigFile = "config.ini";
//static const char* strNetwork = "NETWORK";
@@ -42,6 +43,7 @@
static const char* strAltitude = "ALTITUDE";
static const char* strRTL = "RTL";
static const char* strHover = "HOVER";
+static const char* strTurns = "TURNS";
static const char* strMode = "MODE";

#if (NETWORK_INTERFACE != NETWORK_INTERFACE_NONE)
@@ -141,6 +143,39 @@
# NONE = 0, FULL = 1, PITCH = 2
waypoint = 1
*/
+
+
+/*
+
+new for helicalTurns:
+
+#define FEED_FORWARD 1.0
+#define TURN_RATE_NAV 30.0
+#define TURN_RATE_FBW 60.0
+#define CRUISE_SPEED 12.0
+#define ANGLE_OF_ATTACK_NORMAL -0.8
+#define ANGLE_OF_ATTACK_INVERTED -7.2
+#define ELEVATOR_TRIM_NORMAL -0.03
+#define ELEVATOR_TRIM_INVERTED -0.67
+
+and we remove the following:
+#define AILERON_BOOST 0.8
+#define RUDDER_ELEV_MIX 0.2
+#define ROLL_ELEV_MIX 0.35
+
+ */
+static void load_turns(void)
+{
+ turns.FeedForward = ini_getf(strTurns, "feedfwd", FEED_FORWARD,
strConfigFile);
+ DPRINT("turns.FeedForward = %f\r\n", turns.FeedForward);
+ turns.TurnRateNav = ini_getf(strTurns, "ratenav", TURN_RATE_NAV,
strConfigFile);
+ turns.TurnRateFBW = ini_getf(strTurns, "ratefbw", TURN_RATE_FBW,
strConfigFile);
+ turns.CruiseSpeed = ini_getf(strTurns, "crsespd", CRUISE_SPEED,
strConfigFile);
+ turns.AngleOfAttackNormal = ini_getf(strTurns, "aoanorm",
ANGLE_OF_ATTACK_NORMAL, strConfigFile);
+ turns.AngleOfAttackInverted = ini_getf(strTurns, "aoainvt",
ANGLE_OF_ATTACK_INVERTED, strConfigFile);
+ turns.ElevatorTrimNormal = ini_getf(strTurns, "elenorm",
ELEVATOR_TRIM_NORMAL, strConfigFile);
+ turns.ElevatorTrimInverted = ini_getf(strTurns, "eleinvt",
ELEVATOR_TRIM_INVERTED, strConfigFile);
+}

static void load_gains(void)
{
@@ -149,13 +184,13 @@
gains.RollKD = ini_getf(strRoll, "rollkd", ROLLKD, strConfigFile);
gains.YawKPAileron = ini_getf(strRoll, "yawkp", YAWKP_AILERON,
strConfigFile);
gains.YawKDAileron = ini_getf(strRoll, "yawkd", YAWKD_AILERON,
strConfigFile);
- gains.AileronBoost = ini_getf(strRoll, "boost", AILERON_BOOST,
strConfigFile);
+// gains.AileronBoost = ini_getf(strRoll, "boost", AILERON_BOOST,
strConfigFile);

// Elevator/Pitch Control Gains
gains.Pitchgain = ini_getf(strPitch, "gain", PITCHGAIN, strConfigFile);
gains.PitchKD = ini_getf(strPitch, "pitchkd", PITCHKD, strConfigFile);
- gains.RudderElevMix = ini_getf(strPitch, "rudder", RUDDER_ELEV_MIX,
strConfigFile);
- gains.RollElevMix = ini_getf(strPitch, "roll", ROLL_ELEV_MIX,
strConfigFile);
+// gains.RudderElevMix = ini_getf(strPitch, "rudder", RUDDER_ELEV_MIX,
strConfigFile);
+// gains.RollElevMix = ini_getf(strPitch, "roll", ROLL_ELEV_MIX,
strConfigFile);
gains.ElevatorBoost = ini_getf(strPitch, "boost", ELEVATOR_BOOST,
strConfigFile);
// = ini_getf(strPitch, "invert", INVERTED_NEUTRAL_PITCH, strConfigFile);

@@ -247,11 +282,24 @@
ini_putf(strHover, "wp", hover.HoverPitchTowardsWP, strConfigFile);
ini_putf(strHover, "radius", hover.HoverNavMaxPitchRadius, strConfigFile);
}
+
+static void save_turns(void)
+{
+ ini_putf(strTurns, "feedfwd", turns.FeedForward, strConfigFile);
+ ini_putf(strTurns, "ratenav", turns.TurnRateNav, strConfigFile);
+ ini_putf(strTurns, "ratefbw", turns.TurnRateFBW, strConfigFile);
+ ini_putf(strTurns, "crsespd", turns.CruiseSpeed, strConfigFile);
+ ini_putf(strTurns, "aoanorm", turns.AngleOfAttackNormal, strConfigFile);
+ ini_putf(strTurns, "aoainvt", turns.AngleOfAttackInverted, strConfigFile);
+ ini_putf(strTurns, "elenorm", turns.ElevatorTrimNormal, strConfigFile);
+ ini_putf(strTurns, "eleinvt", turns.ElevatorTrimInverted, strConfigFile);
+}

void config_load(void)
{
load_settings();
load_gains();
+ load_turns();

init_yawCntrl();
init_rollCntrl();
@@ -279,6 +327,7 @@

save_settings();
save_gains();
+ save_turns();
}

void config_init(void)
=======================================
--- /trunk/MatrixPilot/config.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/config.h Sat Aug 1 04:50:37 2015 UTC
@@ -107,7 +107,6 @@
};

struct hover_variables {
-
float HoverRollKP;
float HoverRollKD;
float HoverPitchGain;
@@ -120,9 +119,29 @@
float HoverNavMaxPitchRadius;
};

+struct turns_variables {
+// float FeedFwd;
+// float RateNav;
+// float RateFbw;
+// float CrseSpd;
+// float AoANorm;
+// float AoAInvt;
+// float ElvNorm;
+// float ElvInvt;
+ float FeedForward;
+ float TurnRateNav;
+ float TurnRateFBW;
+ float CruiseSpeed;
+ float AngleOfAttackNormal;
+ float AngleOfAttackInverted;
+ float ElevatorTrimNormal;
+ float ElevatorTrimInverted;
+};
+
extern struct gains_variables gains;
extern struct altit_variables altit;
extern struct hover_variables hover;
+extern struct turns_variables turns;
extern union settings_word settings;
//extern union network_module_word network_modules;

=======================================
--- /trunk/MatrixPilot/config_tests.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/config_tests.c Sat Aug 1 04:50:37 2015 UTC
@@ -233,3 +233,13 @@
#if ((HILSIM_USB == 1) && (BOARD_TYPE != AUAV3_BOARD))
#error("HILSIM_USB only supported on AUAV3 board"
#endif
+
+#ifdef INVERTED_NEUTRAL_PITCH
+#ifdef ANGLE_OF_ATTACK_INVERTED
+#error ( "Both INVERTED_NEUTRAL_PITCH and ANGLE_OF_ATTACK_INVERTED are
being used. Use only one or the other."
+#endif // ANGLE_OF_ATTACK_INVERTED
+#endif // INVERTED_NEUTRAL_PITCH
+
+#if ( AIRFRAME_TYPE == AIRFRAME_HELI )
+ #error("Helical controls does not support AIRFRAME_HELI."
+#endif // AIRFRAME_HELI
=======================================
--- /trunk/MatrixPilot/console.c Sat May 16 13:41:36 2015 UTC
+++ /trunk/MatrixPilot/console.c Sat Aug 1 04:50:37 2015 UTC
@@ -147,11 +147,11 @@
printf("YAWKD_AILERON: %f\r\n", (double)gains.YawKDAileron);
printf("ROLLKP: %f\r\n", (double)gains.RollKP);
printf("ROLLKD: %f\r\n", (double)gains.RollKD);
- printf("AILERON_BOOST: %f\r\n", (double)gains.AileronBoost);
+// printf("AILERON_BOOST: %f\r\n", (double)gains.AileronBoost);
printf("PITCHGAIN: %f\r\n", (double)gains.Pitchgain);
printf("PITCHKD: %f\r\n", (double)gains.PitchKD);
- printf("RUDDER_ELEV_MIX: %f\r\n", (double)gains.RudderElevMix);
- printf("ROLL_ELEV_MIX: %f\r\n", (double)gains.RollElevMix);
+// printf("RUDDER_ELEV_MIX: %f\r\n", (double)gains.RudderElevMix);
+// printf("ROLL_ELEV_MIX: %f\r\n", (double)gains.RollElevMix);
printf("ELEVATOR_BOOST: %f\r\n", (double)gains.ElevatorBoost);
printf("YAWKP_RUDDER: %f\r\n", (double)gains.YawKPRudder);
printf("YAWKD_RUDDER: %f\r\n", (double)gains.YawKDRudder);
=======================================
--- /trunk/MatrixPilot/example-options-files/options.EasyStar-Bill.h Sat
Sep 13 11:36:23 2014 UTC
+++ /trunk/MatrixPilot/example-options-files/options.EasyStar-Bill.h Sat
Aug 1 04:50:37 2015 UTC
@@ -437,6 +437,19 @@
#define TRIGGER_PULSE_DURATION 250
#define TRIGGER_REPEAT_PERIOD 4000

+// FEED_FORWARD is a feed forward gain for deflecting control surfaces for
turn rate.
+// The KP gains for each axis are multiplied by FEED_FORWARD to determine
+// the feed forward gain for that axis.
+// For each axis, a deflection term is added equal to the feed forward
gain for that axis
+// times projection of the desired earth vertical rotation rate onto that
axis
+#define FEED_FORWARD 1.0
+
+// TURN_RATE_NAV and TURN_RATE_FBW set the gains of the helical turn
control for
+// waypoint navigation mode and fly by wire mode respectively.
+// They are specified in terms of the maximum desired turning rate in
degrees per second in each mode.
+// The largest possible value is 240 degrees per second, anything larger
will be clipped to 240.
+#define TURN_RATE_NAV 30.0
+#define TURN_RATE_FBW 60.0


////////////////////////////////////////////////////////////////////////////////
// Control gains.
=======================================
--- /trunk/MatrixPilot/flightplan-waypoints.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/flightplan-waypoints.c Sat Aug 1 04:50:37 2015 UTC
@@ -54,7 +54,7 @@
static struct waypointDef* currentWaypointSet = (struct
waypointDef*)WaypointSet;
static int16_t numPointsInCurrentSet = 0;
#else
-static struct waypointDef* currentWaypointSet = (struct
waypointDef*)waypoints;
+static const struct waypointDef* currentWaypointSet = (struct
waypointDef*)waypoints;
static int16_t numPointsInCurrentSet = NUMBER_POINTS;
#endif

=======================================
--- /trunk/MatrixPilot/gain_variables.h Mon Sep 23 03:37:09 2013 UTC
+++ /trunk/MatrixPilot/gain_variables.h Sat Aug 1 04:50:37 2015 UTC
@@ -54,7 +54,7 @@
// ELEVATOR_BOOST is the additional gain multiplier for the manually
commanded elevator deflection
extern uint16_t pitchgain;
extern uint16_t pitchkd;
-extern uint16_t rudderElevMixGain;
+//extern uint16_t rudderElevMixGain;
extern uint16_t rollElevMixGain;
//#define ELEVATOR_BOOST 0.5

=======================================
--- /trunk/MatrixPilot/helicalTurnCntrl.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/helicalTurnCntrl.c Sat Aug 1 04:50:37 2015 UTC
@@ -32,7 +32,465 @@
#include "../libDCM/rmat.h"
#include <math.h>

+//#ifndef RTL_PITCH_DOWN
+//#define RTL_PITCH_DOWN (0.0)
+//#endif // RLT_PITCH_DOWN
+
+//#ifndef ANGLE_OF_ATTACK_NORMAL
+//#define ANGLE_OF_ATTACK_NORMAL (0.0)
+//#endif // ANGLE_OF_ATTACK_NORMAL
+
+//#ifndef ANGLE_OF_ATTACK_INVERTED
+//#define ANGLE_OF_ATTACK_INVERTED (0.0)
+//#endif // ANGLE_OF_ATTACK_INVERTED
+
+//#ifndef ELEVATOR_TRIM_NORMAL
+//#define ELEVATOR_TRIM_NORMAL (0.0)
+//#endif // ELEVATOR_TRIM_NORMAL
+
+//#ifndef ELEVATOR_TRIM_INVERTED
+//#define ELEVATOR_TRIM_INVERTED (0.0)
+//#endif // ELEVATOR_TRIM_INVERTED
+
+//#ifndef CRUISE_SPEED
+//#define CRUISE_SPEED (12.0)
+//#endif // CRUISE_SPEED
+
+#ifndef INVERTED_NEUTRAL_PITCH
+#define INVERTED_NEUTRAL_PITCH (0.0)
+#endif
+
+#define RTLKICK ((int32_t)(gains.RtlPitchDown*(RMAX/57.3)))
+#define INVNPITCH ((int32_t)(INVERTED_NEUTRAL_PITCH*(RMAX/57.3)))
+#define AOA_NORMAL
((int16_t)(turns.AngleOfAttackNormal*(RMAX/57.3)))
+#define AOA_INVERTED
((int16_t)(turns.AngleOfAttackInverted*(RMAX/57.3)))
+#define ELEV_TRIM_NORMAL ((int16_t)SERVORANGE*turns.ElevatorTrimNormal)
+#define ELEV_TRIM_INVERTED ((int16_t)SERVORANGE*turns.ElevatorTrimInverted)
+#define STALL_SPEED_CM_SEC ((uint16_t)turns.CruiseSpeed*50.0) // assume
stall speed approximately 1/2 of cruise speed
+
+#define AOA_OFFSET ((int16_t)((AOA_NORMAL + AOA_INVERTED)/2)) //
offset is the average of the two values
+#define AOA_SLOPE ((int16_t)((AOA_NORMAL - AOA_INVERTED) * 4))
// multiply by 4 because base speed is 1/2 of cruise
+#define ELEVATOR_TRIM_OFFSET ((int16_t)((ELEV_TRIM_NORMAL +
ELEV_TRIM_INVERTED)/2)) // offset is the average of the two values
+#define ELEVATOR_TRIM_SLOPE ((int16_t)((ELEV_TRIM_NORMAL -
ELEV_TRIM_INVERTED) * 4)) // multiply by 4 because base speed is 1/2 of
cruise
+
+#define GRAVITYCMSECSEC (981)
+#define RADSTOGYRO ((uint16_t)48*SCALEGYRO) // used in the
conversion from radians per second to raw gyro units
+
+#define MAX_INPUT (1000) // maximum input in pwm units
+
+#define MINIMUM_AIRSPEED (500) // minimum value of airspeed in cm/sec
to be used in tilt computation,
+ // mainly used for ground testing of
turning tilt, which would go to zero at zero airspeed
+
+int16_t tiltError[3];
+int16_t desiredRotationRateRadians[3];
+int16_t rotationRateError[3];
+//int16_t angleOfAttack;
+
+static int16_t estimatedLift;
+static int16_t relativeLoading;
+
+// Compute estimated wing lift based on orientation.
+// This information can be determined directly from the accelerometers,
+// but this creates an unstable feedback loop through the elevator.
+// It is better to compute the lift as a feed forward term.
+// It can be shown that wing lift divided by mass times gravity during a
helical turn is
+// equal to (Z + X * (X/Z)), where X, Y, and Z are the 3 elements
+// of the bottom row of the direction cosine matrix, in real (floating
point) values.
+// Note: In principle, X/Z can be computed from X and Z, but since X, Z,
and X/Z are already available
+// in the helical turn control computations, it is more efficient to
supply X/Z rather than recompute it.
+// The computation of lift can use a mix of actual and desired values of
X, Z and X/Z.
+// The selection of the mix of actual and desired values for X, Z and X/Z
is done when the routine is called.
+// The following routine computes 1/16 of the ratio of wing loading
divided by mass*gravity.
+// The scale factor of 1/16 was selected to handle a fractional
representation of a maximum lift/mg of more than 4.
+
+static int16_t wingLift(int16_t X, int16_t Z, int16_t XoverZ)
+{
+ // compute (1/16*(Z + X *(X/Z)))*2^16
+ union longww lift;
+ union longww accum;
+
+ // compute the first term. X is already scaled by 1/4, so apply another
1/4:
+
+ accum._.W1 = Z;
+ accum._.W0 = 0;
+ accum.WW = accum.WW >> 2; // divide by 4
+ lift.WW = accum.WW; // Z/16
+
+ // compute the second term in result, X *(X/Z)
+ // X has been divided by 4
+ // X/Z has been divided by 8
+ // so we need to multiply by 2
+ accum.WW = __builtin_mulss(X, XoverZ);
+ accum.WW += accum.WW; // multiply by 2
+
+ // add the two terms
+ lift.WW += accum.WW;
+
+ return lift._.W1;
+}
+
+// Compute relative wing loading, 2**15*(Load/(mass*Gravity))*(V0/V)**2
+// This number ranges from -2**15 to +2**15. Either extreme represents
stall conditions.
+// Typical values for relative wing loading under normal conditions are
around 1/8 to 1/4 of the stall value.
+// V0 is stall speed in centimeters per second during level flight, V is
airspeed in centimeters per second.
+// Load is wing loading in acceleration units, G is gravity.
+// Relative wing loading is 2**15 when airspeed is equal to stall speed
during level, unaccelerated flight
+//
+// Implement as 8*((2**16)*(a/(16g))*(V0/V)**2
+//
+
+static int16_t relativeWingLoading(int16_t wingLoad, uint16_t airSpeed)
+{
+ // wingLoad is(2**16)*((wing_load / mass*gravity) / 16)
+ // stallSpeed is the stall speed in centimeters per second
+ // airSpeed is the air speed in centimeters per second
+
+ uint16_t stallSpeed = STALL_SPEED_CM_SEC;
+ int16_t result = 0;
+ uint32_t long_unsigned_accum;
+ union longww long_signed_accum;
+ uint16_t unsigned_accum;
+
+ // if airspeed is less than or equal to stall speed, return zero
+ if (airSpeed <= stallSpeed)
+ {
+ return 0;
+ }
+
+ long_unsigned_accum = (uint32_t)stallSpeed;
+ long_unsigned_accum = long_unsigned_accum << 16; //(2**16)*V0, 32 bits
unsigned
+ unsigned_accum = __builtin_divud(long_unsigned_accum, airSpeed);
//(2**16)*(V0/V), 16 bits unsigned
+ long_unsigned_accum = __builtin_muluu(unsigned_accum, unsigned_accum);
//(2**32)*(V0/V)**2, 32 bits unsigned
+ unsigned_accum = long_unsigned_accum >> 16; //(2**16)*(V0/V)**2, 16 bits
unsigned
+ long_signed_accum.WW = __builtin_mulus(unsigned_accum, wingLoad);
//(2**32)*(a/16g)*(V0/V)**2, 32 bits unsigned
+ if (abs(long_signed_accum._.W1) < 4095)
+ {
+ long_signed_accum.WW = long_signed_accum.WW << 3; // multiply by 8
+ result = long_signed_accum._.W1;
+ }
+ else
+ {
+ if (wingLoad > 0)
+ {
+ result = 32767;
+ }
+ else
+ {
+ result = -32767;
+ }
+ }
+ return result;
+}
+
+
+// helicalTurnCntrl determines the values of the elements of the bottom
row of rmat
+// as well as the required rotation rates in the body frame that are
required to make a coordinated turn.
+// The required values for the bottom row of rmat are placed in the vector
desiredTilt.
+// Desired tilt is computed from the helical turn parameters from desired
climb rate, turn rate, and airspeed.
+// The cross product of rmat[6,7,8] with the desiredTilt produces the
orientation error.
+// The desired rotation rate in the body frame is computed by multiplying
desired turn rate times actual tilt vector.
+// The rotation rate error is the actual rotation rate vector in the body
frame minus the desired rotation rate vector.
+// The tilt and rate vectors are then used by roll, pitch, and yaw control
to deflect control surfaces.

void helicalTurnCntrl(void)
{
+ union longww accum;
+ int16_t pitchAdjustAngleOfAttack;
+ int16_t rollErrorVector[3];
+ int16_t rtlkick;
+ int16_t desiredPitch;
+ int16_t steeringInput;
+ int16_t desiredTurnRateRadians;
+ int16_t desiredTiltVector[3];
+ int16_t desiredRotationRateGyro[3];
+ uint16_t airSpeed;
+ union longww desiredTilt;
+ int16_t desiredPitchVector[2];
+ int16_t desiredPerpendicularPitchVector[2];
+ int16_t actualPitchVector[2];
+ int16_t pitchDot;
+ int16_t pitchCross;
+ int16_t pitchError;
+ int16_t pitchEarthBodyProjection[2];
+ int16_t angleOfAttack;
+#ifdef TestGains
+ state_flags._.GPS_steering = 0; // turn off navigation
+ state_flags._.pitch_feedback = 1; // turn on stabilization
+ airSpeed = 981; // for testing purposes, an airspeed is needed
+#else
+ airSpeed = air_speed_3DIMU;
+ if (airSpeed < MINIMUM_AIRSPEED) airSpeed = MINIMUM_AIRSPEED;
+#endif
+
+ // determine the desired turn rate as the sum of navigation and fly by
wire.
+ // this allows the pilot to override navigation if needed.
+ steeringInput = 0 ; // just in case no airframe type is specified or
radio is off
+ if (udb_flags._.radio_on == 1)
+ {
+#if (AIRFRAME_TYPE == AIRFRAME_STANDARD)
+ if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about
this
+ {
+ steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[
AILERON_INPUT_CHANNEL ];
+ steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED,
steeringInput);
+ }
+ else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
+ {
+ steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[
RUDDER_INPUT_CHANNEL ];
+ steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED,
steeringInput);
+ }
+ else
+ {
+ steeringInput = 0;
+ }
+#endif // AIRFRAME_STANDARD
+
+#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
+ // use aileron channel if it is available, otherwise use rudder
+ if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about
this
+ {
+ steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] -
udb_pwTrim[AILERON_INPUT_CHANNEL];
+ steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED,
steeringInput);
+ }
+ else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
+ {
+ // unmix the Vtail
+ int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED,
(udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
+ int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED,
(udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
+ steeringInput = (-rudderInput + elevatorInput);
+ }
+ else
+ {
+ steeringInput = 0;
+ }
+#endif // AIRFRAME_VTAIL
+
+#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
+ // delta wing must have an aileron input, so use that
+ // unmix the elevons
+ int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED,
(udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
+ int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED,
(udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
+ steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
((elevatorInput - aileronInput)));
+#endif // AIRFRAME_DELTA
+ }
+
+ if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT;
+ if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT;
+
+ // note that total steering is the sum of pilot input and waypoint
navigation,
+ // so that the pilot always has some say in the matter
+
+ accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT);
+
+ if ((settings._.AileronNavigation || settings._.RudderNavigation) &&
state_flags._.GPS_steering)
+ {
+ accum.WW +=(int32_t) navigate_determine_deflection('t');
+ }
+
+ if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t)
2*(int32_t) RMAX - 1;
+ if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t)
2*(int32_t) RMAX + 1;
+
+ desiredTurnRateRadians = accum._.W0;
+
+ // compute the desired tilt from desired turn rate and air speed
+ // range for acceleration is plus minus 4 times gravity
+ // range for turning rate is plus minus 4 radians per second
+
+ // desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for
the turn
+ // desiredTilt = desiredTurnRate * airSpeed / gravity
+ // desiredTilt = RMAX/2*"real desired tilt"
+ // desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate
in radians per second
+ // airSpeed is air speed centimeters per second
+ // gravity is 981 centimeters per second per second
+
+ desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed);
+ desiredTilt.WW /= GRAVITYCMSECSEC;
+
+ // limit the lateral acceleration to +- 4 times gravity, total wing
loading approximately 4.12 times gravity
+
+ if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1)
+ {
+ desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1;
+ accum.WW = __builtin_mulsu(desiredTilt._.W0, GRAVITYCMSECSEC);
+ accum.WW /= airSpeed;
+ desiredTurnRateRadians = accum._.W0;
+ }
+ else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1)
+ {
+ desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1;
+ accum.WW = __builtin_mulsu(desiredTilt._.W0, GRAVITYCMSECSEC);
+ accum.WW /= airSpeed;
+ desiredTurnRateRadians = accum._.W0;
+ }
+
+ // Compute the amount of lift needed to perform the desired turn
+ // Tests show that the best estimate of lift is obtained using
+ // actual values of rmat[6] and rmat[8], and the commanded value of their
ratio
+ estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0);
+
+ // compute angle of attack and elevator trim based on relative wing
loading.
+ // relative wing loading is the ratio of wing loading divided by the
stall wing loading, as a function of air speed
+ // both angle of attack and trim are computed by a linear approximation
as a function of relative loading:
+ // y = (2m)*(x/2) + b, y is either angle of attack or elevator trim.
+ // x is relative wing loading. (x/2 is computed instead of x)
+ // 2m and b are determined from values of angle of attack and trim at
stall speed, normal and inverted.
+ // b = (y_normal + y_inverted) / 2.
+ // 2m = (y_normal - y_inverted).
+
+ // If airspeed is greater than stall speed, compute angle of attack and
elevator trim,
+ // otherwise set AoA and trim to zero.
+
+ if (air_speed_3DIMU > STALL_SPEED_CM_SEC)
+ {
+ // compute "x/2", the relative wing loading
+ relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU);
+
+ // multiply x/2 by 2m for angle of attack
+ accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading);
+ // add mx to b
+ angleOfAttack = AOA_OFFSET + accum._.W1;
+
+ // project angle of attack into the earth frame
+ accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2;
+ pitchAdjustAngleOfAttack = accum._.W1;
+
+ // similarly, compute elevator trim
+ accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading);
+ elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1;
+ }
+ else
+ {
+ angleOfAttack = 0;
+ pitchAdjustAngleOfAttack = 0;
+ elevatorLoadingTrim = 0;
+ }
+// SetAofA(angleOfAttack); // removed by helicalTurns
+
+ // convert desired turn rate from radians/second to gyro units
+
+ accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn
rate in radians times 16 to provide resolution for the divide to follow
+ accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times
the required gyro signal for the turn.
+
+ // compute desired rotation rate vector in body frame, scaling is same as
gyro signal
+
+ VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this
operation has side effect of dividing by 2
+
+ // compute desired rotation rate vector in body frame, scaling is in
RMAX/2*radians/sec
+
+ VectorScale(3, desiredRotationRateRadians, &rmat[6],
desiredTurnRateRadians); // this produces half of what we want
+ VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians,
desiredRotationRateRadians); // double
+
+ // incorporate roll into desired tilt vector
+
+ desiredTiltVector[0] = desiredTilt._.W0;
+ desiredTiltVector[1] = 0;
+ desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the
RMAX/2 scaling in both tilt and rotation rate
+ vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure
tilt vector has magnitude RMAX
+
+ // incorporate pitch into desired tilt vector
+ // compute return to launch pitch down kick for unpowered RTL
+ if (!udb_flags._.radio_on && state_flags._.GPS_steering)
+ {
+ rtlkick = RTLKICK;
+ }
+ else
+ {
+ rtlkick = 0;
+ }
+
+ // Compute Matt's glider pitch adjustment
+#if (GLIDE_AIRSPEED_CONTROL == 1)
+ fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
+#endif
+
+ // Compute total desired pitch
+#if (GLIDE_AIRSPEED_CONTROL == 1)
+ desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust;
+#else
+ desiredPitch = - rtlkick + pitchAltitudeAdjust;
+#endif
+
+ // Adjustment for inverted flight
+ if (!canStabilizeInverted() || !desired_behavior._.inverted)
+ {
+ // normal flight
+ desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack;
+ }
+ else
+ {
+ // inverted flight
+ desiredTiltVector[0] = - desiredTiltVector[0];
+ desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack -
INVNPITCH; // only one of the adjustments is not zero
+ desiredTiltVector[2] = - desiredTiltVector[2];
+ }
+
+ vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure
tilt vector has magnitude RMAX
+
+ // compute roll error
+
+ VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute
tilt orientation error
+ if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90
degree error
+ {
+ vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90
degrees, make the tilt error vector parallel to desired axis, with
magnitude RMAX
+ }
+
+ tiltError[1] = rollErrorVector[1];
+
+ // compute pitch error
+
+ // start by computing the projection of earth frame pitch error to body
frame
+
+ pitchEarthBodyProjection[0] = rmat[6];
+ pitchEarthBodyProjection[1] = rmat[8];
+
+ // normalize the projection vector and compute the cosine of the actual
pitch as a side effect
+
+ actualPitchVector[1] =(int16_t)
vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection);
+
+ // complete the actual pitch vector
+
+ actualPitchVector[0] = rmat[7];
+
+ // compute the desired pitch vector
+
+ desiredPitchVector[0] = - desiredPitch;
+ desiredPitchVector[1] = RMAX;
+ vector2_normalize(desiredPitchVector, desiredPitchVector);
+
+ // rotate desired pitch vector by 90 degrees to be able to compute cross
product using VectorDot
+
+ desiredPerpendicularPitchVector[0] = desiredPitchVector[1];
+ desiredPerpendicularPitchVector[1] = - desiredPitchVector[0];
+
+ // compute pitchDot, the dot product of actual and desired pitch vector
+ // (the 2* that appears in several of the following expressions is a
result of the Q2.14 format)
+
+ pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector);
+
+ // compute pitchCross, the cross product of the actual and desired pitch
vector
+
+ pitchCross = 2*VectorDotProduct(2, actualPitchVector,
desiredPerpendicularPitchVector);
+
+ if (pitchDot > 0)
+ {
+ pitchError = pitchCross;
+ }
+ else
+ {
+ if (pitchCross > 0)
+ {
+ pitchError = RMAX;
+ }
+ else
+ {
+ pitchError = - RMAX;
+ }
+ }
+
+ // multiply the normalized rmat[6], rmat[8] vector by the pitch error
+ VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection,
pitchError);
+ tiltError[0] = 2*pitchEarthBodyProjection[1];
+ tiltError[2] = - 2*pitchEarthBodyProjection[0];
+
+ // compute the rotation rate error vector
+ VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro);
}
=======================================
--- /trunk/MatrixPilot/helicalTurnCntrl.h Wed May 6 15:10:19 2015 UTC
+++ /trunk/MatrixPilot/helicalTurnCntrl.h Sat Aug 1 04:50:37 2015 UTC
@@ -19,4 +19,9 @@
// along with MatrixPilot. If not, see <http://www.gnu.org/licenses/>.


+extern int16_t tiltError[3];
+extern int16_t desiredRotationRateRadians[3];
+extern int16_t rotationRateError[3];
+//extern int16_t angleOfAttack;
+
void helicalTurnCntrl(void);
=======================================
--- /trunk/MatrixPilot/navigate.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/navigate.c Sat Aug 1 04:50:37 2015 UTC
@@ -48,8 +48,11 @@

uint16_t yawkpail; // only exported for parameter_table
uint16_t yawkprud; // only exported for parameter_table
+uint16_t turngainfbw; // fly by wire turn gain
+uint16_t turngainnav; // waypoints turn gain

int16_t tofinish_line = 0;
+int16_t progress_to_goal = 0;
int8_t desired_dir = 0;
int8_t extended_range = 0;

@@ -68,6 +71,9 @@
static struct waypointparameters navgoal;
static int16_t desired_bearing_over_ground_vector[2];

+struct relative2D togoal = { 0, 0 };
+int8_t desired_bearing_over_ground;
+
int16_t navigate_get_goal(vect3_16t* _goal)
{
if (_goal != NULL)
@@ -83,6 +89,8 @@
{
yawkpail = (uint16_t)(gains.YawKPAileron*RMAX);
yawkprud = (uint16_t)(gains.YawKPRudder*RMAX);
+ turngainnav = (uint16_t)((turns.TurnRateNav/57.3)*RMAX);
+ turngainfbw = (uint16_t)((turns.TurnRateFBW/57.3)*RMAX);
}

void save_navigation(void)
@@ -451,13 +459,9 @@
vector2_normalize(actualXY, actualXY);
actualX = actualXY[0];
actualY = actualXY[1];
- if (navType == 'y')
+ if (navType == 't')
{
- yawkp = yawkprud;
- }
- else if (navType == 'a')
- {
- yawkp = yawkpail;
+ yawkp = turngainnav;
}
else if (navType == 'h')
{
@@ -470,15 +474,9 @@
}
else
{
- if (navType == 'y')
- {
- yawkp = yawkprud ;
- actualX = rmat[1];
- actualY = rmat[4];
- }
- else if (navType == 'a')
+ if (navType == 't')
{
- yawkp = yawkpail;
+ yawkp = turngainnav;
actualX = rmat[1];
actualY = rmat[4];
}
@@ -523,7 +521,11 @@
{
deflectionAccum.WW = -deflectionAccum.WW;
}
- // multiply by wind gain adjustment, and multiply by 2
- deflectionAccum.WW = (__builtin_mulsu(deflectionAccum._.W1, wind_gain) <<
1);
+
+#if (WIND_GAIN_ADJUSTMENT == 1)
+#error ( "wind gain adjustment is under construction and is not working at
this time.")
+ deflectionAccum.WW = __builtin_mulsu(deflectionAccum._.W1, wind_gain);
+#endif // WIND_GAIN_ADJUSTMENT
+
return deflectionAccum._.W1;
}
=======================================
--- /trunk/MatrixPilot/navigate.h Fri Apr 3 00:24:45 2015 UTC
+++ /trunk/MatrixPilot/navigate.h Sat Aug 1 04:50:37 2015 UTC
@@ -25,6 +25,8 @@
extern int16_t tofinish_line;
extern int8_t extended_range;
extern int8_t desired_dir;
+extern uint16_t turngainfbw; // fly by wire turn gain
+extern uint16_t turngainnav; // waypoints turn gain

void init_navigation(void);
void save_navigation(void);
=======================================
--- /trunk/MatrixPilot/nv_memory_table.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/nv_memory_table.c Sat Aug 1 04:50:37 2015 UTC
@@ -7,12 +7,13 @@
#if(USE_NV_MEMORY == 1)

const mavlink_parameter_block mavlink_parameter_blocks[] = {
- { STORAGE_HANDLE_CONTROL_GAINS, 0, 12, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB, NULL },
- { STORAGE_HANDLE_MAG_CALIB, 12, 10, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB, NULL },
- { STORAGE_HANDLE_RADIO_TRIM, 22, 15, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB,
&udb_skip_radio_trim },
- { STORAGE_HANDLE_IMU_CALIB, 37, 7, STORAGE_FLAG_LOAD_AT_REBOOT |
STORAGE_FLAG_STORE_CALIB, &udb_skip_imu_calibration },
- { STORAGE_HANDLE_THROTTLE_HEIGHT_OPTIONS, 44, 9,
STORAGE_FLAG_LOAD_AT_STARTUP | STORAGE_FLAG_LOAD_AT_REBOOT, NULL },
- { STORAGE_HANDLE_AIRSPEED_OPTIONS, 53, 10, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT, NULL },
+ { STORAGE_HANDLE_CONTROL_GAINS, 0, 11, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB, NULL },
+ { STORAGE_HANDLE_MAG_CALIB, 11, 10, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB, NULL },
+ { STORAGE_HANDLE_RADIO_TRIM, 21, 15, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT | STORAGE_FLAG_STORE_CALIB,
&udb_skip_radio_trim },
+ { STORAGE_HANDLE_IMU_CALIB, 36, 7, STORAGE_FLAG_LOAD_AT_REBOOT |
STORAGE_FLAG_STORE_CALIB, &udb_skip_imu_calibration },
+ { STORAGE_HANDLE_THROTTLE_HEIGHT_OPTIONS, 43, 9,
STORAGE_FLAG_LOAD_AT_STARTUP | STORAGE_FLAG_LOAD_AT_REBOOT, NULL },
+ { STORAGE_HANDLE_AIRSPEED_OPTIONS, 52, 10, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT, NULL },
+ { STORAGE_HANDLE_TURNS_OPTIONS, 62, 8, STORAGE_FLAG_LOAD_AT_STARTUP |
STORAGE_FLAG_LOAD_AT_REBOOT, NULL },
};


=======================================
--- /trunk/MatrixPilot/parameter_table.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/parameter_table.c Sat Aug 1 04:50:37 2015 UTC
@@ -15,7 +15,6 @@

#include "gain_variables.h"

-
const mavlink_parameter_parser mavlink_parameter_parsers[] = {
{ &mavlink_send_param_int16, &mavlink_set_param_int16,
MAVLINK_TYPE_INT32_T},
{ &mavlink_send_param_Q14, &mavlink_set_param_Q14, MAVLINK_TYPE_FLOAT},
@@ -41,7 +40,6 @@
{"PID_YAWKDAIL", {.param_float=0.0}, {.param_float=0.5},
UDB_TYPE_GYROSCALE_Q14, PARAMETER_READWRITE, (void*)&yawkdail,
sizeof(yawkdail) },
{"PID_PITCHGAIN", {.param_float=0.0}, {.param_float=0.5}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&pitchgain, sizeof(pitchgain) },
{"PID_PITCHKD", {.param_float=0.0}, {.param_float=0.5}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&pitchkd, sizeof(pitchkd) },
- {"PID_RUDELEVGAIN", {.param_float=0.0}, {.param_float=0.5}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&rudderElevMixGain, sizeof(rudderElevMixGain) },
{"PID_ROLLKPRUD", {.param_float=0.0}, {.param_float=0.5}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&rollkprud, sizeof(rollkprud) },
{"PID_YAWKPRUD", {.param_float=0.0}, {.param_float=0.5}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&yawkprud, sizeof(yawkprud) },
{"PID_YAWKDRUD", {.param_float=0.0}, {.param_float=0.5},
UDB_TYPE_GYROSCALE_Q14, PARAMETER_READWRITE, (void*)&yawkdrud,
sizeof(yawkdrud) },
@@ -59,51 +57,21 @@
{"MAG_OFFSET2", {.param_int32=-32767}, {.param_int32=32767},
UDB_TYPE_INT, PARAMETER_READWRITE, (void*)&udb_magOffset[2],
sizeof(udb_magOffset[2]) },
{"MAG_DECLINATION", {.param_int32=-180}, {.param_int32=180},
UDB_TYPE_INT_CIRCULAR, PARAMETER_READWRITE,
(void*)&dcm_declination_angle.BB, sizeof(dcm_declination_angle.BB) },

- {"PWTRIM_AILERON", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[AILERON_INPUT_CHANNEL], sizeof(
- udb_pwTrim[AILERON_INPUT_CHANNEL]) },
- {"PWTRIM_ELEVATOR", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL], sizeof(
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) },
- {"PWTRIM_RUDDER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[RUDDER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[RUDDER_INPUT_CHANNEL]) },
- {"PWTRIM_AILERON2", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL], sizeof(
- udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL]) },
- {"PWTRIM_ROLL", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[ROLL_INPUT_CHANNEL], sizeof(
- udb_pwTrim[ROLL_INPUT_CHANNEL]) },
- {"PWTRIM_PITCH", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[PITCH_INPUT_CHANNEL], sizeof(
- udb_pwTrim[PITCH_INPUT_CHANNEL]) },
- {"PWTRIM_THROTTLE", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[THROTTLE_INPUT_CHANNEL], sizeof(
- udb_pwTrim[THROTTLE_INPUT_CHANNEL]) },
- {"PWTRIM_YAW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[YAW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[YAW_INPUT_CHANNEL]) },
- {"PWTRIM_FLAP", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[FLAP_INPUT_CHANNEL], sizeof(
- udb_pwTrim[FLAP_INPUT_CHANNEL]) },
- {"PWTRIM_AIRBRAKE", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[BRAKE_INPUT_CHANNEL], sizeof(
- udb_pwTrim[BRAKE_INPUT_CHANNEL]) },
- {"PWTRIM_SPOILER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[SPOILER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[SPOILER_INPUT_CHANNEL]) },
- {"PWTRIM_CAMBER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMBER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMBER_INPUT_CHANNEL]) },
- {"PWTRIM_CROW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CROW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CROW_INPUT_CHANNEL]) },
- {"PWTRIM_CAMPITCH", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL]) },
- {"PWTRIM_CAM_YAW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL]) },
+ {"PWTRIM_AILERON", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[AILERON_INPUT_CHANNEL],
sizeof(udb_pwTrim[AILERON_INPUT_CHANNEL]) },
+ {"PWTRIM_ELEVATOR", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[ELEVATOR_INPUT_CHANNEL],
sizeof(udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) },
+ {"PWTRIM_RUDDER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[RUDDER_INPUT_CHANNEL],
sizeof(udb_pwTrim[RUDDER_INPUT_CHANNEL]) },
+ {"PWTRIM_AILERON2", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL],
sizeof(udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL]) },
+ {"PWTRIM_ROLL", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[ROLL_INPUT_CHANNEL],
sizeof(udb_pwTrim[ROLL_INPUT_CHANNEL]) },
+ {"PWTRIM_PITCH", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[PITCH_INPUT_CHANNEL],
sizeof(udb_pwTrim[PITCH_INPUT_CHANNEL]) },
+ {"PWTRIM_THROTTLE", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[THROTTLE_INPUT_CHANNEL],
sizeof(udb_pwTrim[THROTTLE_INPUT_CHANNEL]) },
+ {"PWTRIM_YAW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[YAW_INPUT_CHANNEL],
sizeof(udb_pwTrim[YAW_INPUT_CHANNEL]) },
+ {"PWTRIM_FLAP", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[FLAP_INPUT_CHANNEL],
sizeof(udb_pwTrim[FLAP_INPUT_CHANNEL]) },
+ {"PWTRIM_AIRBRAKE", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[BRAKE_INPUT_CHANNEL],
sizeof(udb_pwTrim[BRAKE_INPUT_CHANNEL]) },
+ {"PWTRIM_SPOILER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[SPOILER_INPUT_CHANNEL],
sizeof(udb_pwTrim[SPOILER_INPUT_CHANNEL]) },
+ {"PWTRIM_CAMBER", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[CAMBER_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMBER_INPUT_CHANNEL]) },
+ {"PWTRIM_CROW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[CROW_INPUT_CHANNEL],
sizeof(udb_pwTrim[CROW_INPUT_CHANNEL]) },
+ {"PWTRIM_CAMPITCH", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL]) },
+ {"PWTRIM_CAM_YAW", {.param_float=800.0}, {.param_float=2200.0},
UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL]) },

{"IMU_XACCEL_OFF", {.param_int32=-32767}, {.param_int32=32767},
UDB_TYPE_INT, PARAMETER_READWRITE, (void*)&udb_xaccel.offset,
sizeof(udb_xaccel.offset) },
{"IMU_YACCEL_OFF", {.param_int32=-32767}, {.param_int32=32767},
UDB_TYPE_INT, PARAMETER_READWRITE, (void*)&udb_yaccel.offset,
sizeof(udb_yaccel.offset) },
@@ -123,22 +91,26 @@
{"TH_P_HIGH", {.param_int32=0}, {.param_int32=89}, UDB_TYPE_INT,
PARAMETER_READWRITE, (void*)&alt_hold_pitch_high,
sizeof(alt_hold_pitch_high) },
{"TH_P_RTL_DOWN", {.param_int32=0}, {.param_int32=89}, UDB_TYPE_INT,
PARAMETER_READWRITE, (void*)&rtl_pitch_down, sizeof(rtl_pitch_down) },

- {"ASPD_DESIRED", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_DM, PARAMETER_READWRITE,
(void*)&desiredSpeed, sizeof(desiredSpeed) },
- {"ASPD_MIN_GSPD", {0}, {20000},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&minimum_groundspeed, sizeof(minimum_groundspeed) },
- {"ASPD_MIN", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&minimum_airspeed, sizeof(minimum_airspeed) },
- {"ASPD_MAX", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&maximum_airspeed, sizeof(maximum_airspeed) },
- {"ASPD_CRUISE", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&cruise_airspeed, sizeof(cruise_airspeed) },
+ {"ASPD_DESIRED", {.param_float=0}, {.param_float=300.0},
UDB_TYPE_M_AIRSPEED_TO_DM, PARAMETER_READWRITE, (void*)&desiredSpeed,
sizeof(desiredSpeed) },
+ {"ASPD_MIN_GSPD", {.param_float=0}, {.param_float=20000},
UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&minimum_groundspeed, sizeof(minimum_groundspeed) },
+ {"ASPD_MIN", {.param_float=0}, {.param_float=300.0},
UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE, (void*)&minimum_airspeed,
sizeof(minimum_airspeed) },
+ {"ASPD_MAX", {.param_float=0}, {.param_float=300.0},
UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE, (void*)&maximum_airspeed,
sizeof(maximum_airspeed) },
+ {"ASPD_CRUISE", {.param_float=0}, {.param_float=300.0},
UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE, (void*)&cruise_airspeed,
sizeof(cruise_airspeed) },
{"ASPD_P_MIN_ASPD", {.param_int32=-90}, {.param_int32=90.0},
UDB_TYPE_DCM_ANGLE, PARAMETER_READWRITE, (void*)&airspeed_pitch_min_aspd,
sizeof(airspeed_pitch_min_aspd) },
{"ASPD_P_MAX_ASPD", {.param_int32=-90}, {.param_int32=90.0},
UDB_TYPE_DCM_ANGLE, PARAMETER_READWRITE, (void*)&airspeed_pitch_max_aspd,
sizeof(airspeed_pitch_max_aspd) },
{"ASPD_P_RATE_LIM", {.param_int32=1.0}, {.param_int32=720.0},
UDB_TYPE_FRAME_ANGLERATE, PARAMETER_READWRITE,
(void*)&airspeed_pitch_adjust_rate, sizeof(airspeed_pitch_adjust_rate) },
{"ASPD_P_KI", {.param_float=0.0}, {.param_float=1.0}, UDB_TYPE_Q14,
PARAMETER_READWRITE, (void*)&airspeed_pitch_ki, sizeof(airspeed_pitch_ki) },
{"ASPD_P_KI_LIMIT", {.param_int32=0.0}, {.param_int32=45.0},
UDB_TYPE_DCM_ANGLE, PARAMETER_READWRITE, (void*)&airspeed_pitch_ki_limit,
sizeof(airspeed_pitch_ki_limit) },

+ {"TURN_ELE_TR_NRM", {.param_float=-1.0}, {.param_float=1.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.ElevatorTrimNormal,
sizeof(turns.ElevatorTrimNormal) },
+ {"TURN_ELE_TR_INV", {.param_float=-1.0}, {.param_float=1.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.ElevatorTrimInverted,
sizeof(turns.ElevatorTrimInverted) },
+ {"TURN_CRUISE_SPD", {.param_float=0.0}, {.param_float=999.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.CruiseSpeed,
sizeof(turns.CruiseSpeed) },
+ {"TURN_AOA_NORMAL", {.param_float=-90.0}, {.param_float=90.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.AngleOfAttackNormal,
sizeof(turns.AngleOfAttackNormal) },
+ {"TURN_AOA_INV", {.param_float=-90.0}, {.param_float=90.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.AngleOfAttackInverted,
sizeof(turns.AngleOfAttackInverted) },
+ {"TURN_FEED_FWD", {.param_float=0.0}, {.param_float=100.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.FeedForward,
sizeof(turns.FeedForward) },
+ {"TURN_RATE_NAV", {.param_float=0.0}, {.param_float=100.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.TurnRateNav,
sizeof(turns.TurnRateNav) },
+ {"TURN_RATE_FBW", {.param_float=0.0}, {.param_float=100.0},
UDB_TYPE_FLOAT, PARAMETER_READWRITE, (void*)&turns.TurnRateFBW,
sizeof(turns.TurnRateFBW) },
+
};

const uint16_t count_of_parameters_list = sizeof(mavlink_parameters_list)
/ sizeof(mavlink_parameter);
=======================================
--- /trunk/MatrixPilot/parameter_table.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/parameter_table.h Sat Aug 1 04:50:37 2015 UTC
@@ -107,9 +107,9 @@
// Defines required to complete parameter table if these are not defined in
// options.h

-extern fractional udb_magOffset[3]; // magnetic offset in the body frame
of reference
-extern int16_t magGain[3]; // magnetometer calibration gains
-extern int16_t rawMagCalib[3];
+//extern fractional udb_magOffset[3]; // magnetic offset in the body frame
of reference
+//extern int16_t magGain[3]; // magnetometer calibration gains
+//extern int16_t rawMagCalib[3];

#ifndef AILERON_INPUT_CHANNEL
#define AILERON_INPUT_CHANNEL 0
=======================================
--- /trunk/MatrixPilot/parameter_table2.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/parameter_table2.c Sat Aug 1 04:50:37 2015 UTC
@@ -40,7 +40,6 @@
{"PID_YAWKDAIL", {0.0}, {0.5}, UDB_TYPE_GYROSCALE_Q14,
PARAMETER_READWRITE, (void*)&yawkdail, sizeof(yawkdail) },
{"PID_PITCHGAIN", {0.0}, {0.5}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&pitchgain, sizeof(pitchgain) },
{"PID_PITCHKD", {0.0}, {0.5}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&pitchkd, sizeof(pitchkd) },
- {"PID_RUDELEVGAIN", {0.0}, {0.5}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&rudderElevMixGain, sizeof(rudderElevMixGain) },
{"PID_ROLLKPRUD", {0.0}, {0.5}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&rollkprud, sizeof(rollkprud) },
{"PID_YAWKPRUD", {0.0}, {0.5}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&yawkprud, sizeof(yawkprud) },
{"PID_YAWKDRUD", {0.0}, {0.5}, UDB_TYPE_GYROSCALE_Q14,
PARAMETER_READWRITE, (void*)&yawkdrud, sizeof(yawkdrud) },
@@ -58,51 +57,21 @@
{"MAG_OFFSET2", {-32767}, {32767}, UDB_TYPE_INT, PARAMETER_READWRITE,
(void*)&udb_magOffset[2], sizeof(udb_magOffset[2]) },
{"MAG_DECLINATION", {-180}, {180}, UDB_TYPE_INT_CIRCULAR,
PARAMETER_READWRITE, (void*)&dcm_declination_angle.BB,
sizeof(dcm_declination_angle.BB) },

- {"PWTRIM_AILERON", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[AILERON_INPUT_CHANNEL], sizeof(
- udb_pwTrim[AILERON_INPUT_CHANNEL]) },
- {"PWTRIM_ELEVATOR", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL], sizeof(
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) },
- {"PWTRIM_RUDDER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[RUDDER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[RUDDER_INPUT_CHANNEL]) },
- {"PWTRIM_AILERON2", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL], sizeof(
- udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL]) },
- {"PWTRIM_ROLL", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&
- udb_pwTrim[ROLL_INPUT_CHANNEL], sizeof(
- udb_pwTrim[ROLL_INPUT_CHANNEL]) },
- {"PWTRIM_PITCH", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&
- udb_pwTrim[PITCH_INPUT_CHANNEL], sizeof(
- udb_pwTrim[PITCH_INPUT_CHANNEL]) },
- {"PWTRIM_THROTTLE", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[THROTTLE_INPUT_CHANNEL], sizeof(
- udb_pwTrim[THROTTLE_INPUT_CHANNEL]) },
- {"PWTRIM_YAW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&
- udb_pwTrim[YAW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[YAW_INPUT_CHANNEL]) },
- {"PWTRIM_FLAP", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&
- udb_pwTrim[FLAP_INPUT_CHANNEL], sizeof(
- udb_pwTrim[FLAP_INPUT_CHANNEL]) },
- {"PWTRIM_AIRBRAKE", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[BRAKE_INPUT_CHANNEL], sizeof(
- udb_pwTrim[BRAKE_INPUT_CHANNEL]) },
- {"PWTRIM_SPOILER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[SPOILER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[SPOILER_INPUT_CHANNEL]) },
- {"PWTRIM_CAMBER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMBER_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMBER_INPUT_CHANNEL]) },
- {"PWTRIM_CROW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&
- udb_pwTrim[CROW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CROW_INPUT_CHANNEL]) },
- {"PWTRIM_CAMPITCH", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL]) },
- {"PWTRIM_CAM_YAW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&
- udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL], sizeof(
- udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL]) },
+ {"PWTRIM_AILERON", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[AILERON_INPUT_CHANNEL],
sizeof(udb_pwTrim[AILERON_INPUT_CHANNEL]) },
+ {"PWTRIM_ELEVATOR", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[ELEVATOR_INPUT_CHANNEL],
sizeof(udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) },
+ {"PWTRIM_RUDDER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[RUDDER_INPUT_CHANNEL],
sizeof(udb_pwTrim[RUDDER_INPUT_CHANNEL]) },
+ {"PWTRIM_AILERON2", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL],
sizeof(udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL]) },
+ {"PWTRIM_ROLL", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[ROLL_INPUT_CHANNEL],
sizeof(udb_pwTrim[ROLL_INPUT_CHANNEL]) },
+ {"PWTRIM_PITCH", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[PITCH_INPUT_CHANNEL],
sizeof(udb_pwTrim[PITCH_INPUT_CHANNEL]) },
+ {"PWTRIM_THROTTLE", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[THROTTLE_INPUT_CHANNEL],
sizeof(udb_pwTrim[THROTTLE_INPUT_CHANNEL]) },
+ {"PWTRIM_YAW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[YAW_INPUT_CHANNEL],
sizeof(udb_pwTrim[YAW_INPUT_CHANNEL]) },
+ {"PWTRIM_FLAP", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[FLAP_INPUT_CHANNEL],
sizeof(udb_pwTrim[FLAP_INPUT_CHANNEL]) },
+ {"PWTRIM_AIRBRAKE", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[BRAKE_INPUT_CHANNEL],
sizeof(udb_pwTrim[BRAKE_INPUT_CHANNEL]) },
+ {"PWTRIM_SPOILER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[SPOILER_INPUT_CHANNEL],
sizeof(udb_pwTrim[SPOILER_INPUT_CHANNEL]) },
+ {"PWTRIM_CAMBER", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[CAMBER_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMBER_INPUT_CHANNEL]) },
+ {"PWTRIM_CROW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM, PARAMETER_READWRITE,
(void*)&udb_pwTrim[CROW_INPUT_CHANNEL],
sizeof(udb_pwTrim[CROW_INPUT_CHANNEL]) },
+ {"PWTRIM_CAMPITCH", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL]) },
+ {"PWTRIM_CAM_YAW", {800.0}, {2200.0}, UDB_TYPE_PWTRIM,
PARAMETER_READWRITE, (void*)&udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL],
sizeof(udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL]) },

{"IMU_XACCEL_OFF", {-32767}, {32767}, UDB_TYPE_INT, PARAMETER_READWRITE,
(void*)&udb_xaccel.offset, sizeof(udb_xaccel.offset) },
{"IMU_YACCEL_OFF", {-32767}, {32767}, UDB_TYPE_INT, PARAMETER_READWRITE,
(void*)&udb_yaccel.offset, sizeof(udb_yaccel.offset) },
@@ -122,22 +91,26 @@
{"TH_P_HIGH", {0}, {89}, UDB_TYPE_INT, PARAMETER_READWRITE,
(void*)&alt_hold_pitch_high, sizeof(alt_hold_pitch_high) },
{"TH_P_RTL_DOWN", {0}, {89}, UDB_TYPE_INT, PARAMETER_READWRITE,
(void*)&rtl_pitch_down, sizeof(rtl_pitch_down) },

- {"ASPD_DESIRED", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_DM, PARAMETER_READWRITE,
(void*)&desiredSpeed, sizeof(desiredSpeed) },
- {"ASPD_MIN_GSPD", {0}, {20000},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&minimum_groundspeed, sizeof(minimum_groundspeed) },
- {"ASPD_MIN", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&minimum_airspeed, sizeof(minimum_airspeed) },
- {"ASPD_MAX", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&maximum_airspeed, sizeof(maximum_airspeed) },
- {"ASPD_CRUISE", {0}, {300.0},
- UDB_TYPE_M_AIRSPEED_TO_CM, PARAMETER_READWRITE,
(void*)&cruise_airspeed, sizeof(cruise_airspeed) },
+ {"ASPD_DESIRED", {0}, {300.0}, UDB_TYPE_M_AIRSPEED_TO_DM,
PARAMETER_READWRITE, (void*)&desiredSpeed, sizeof(desiredSpeed) },
+ {"ASPD_MIN_GSPD", {0}, {20000}, UDB_TYPE_M_AIRSPEED_TO_CM,
PARAMETER_READWRITE, (void*)&minimum_groundspeed,
sizeof(minimum_groundspeed) },
+ {"ASPD_MIN", {0}, {300.0}, UDB_TYPE_M_AIRSPEED_TO_CM,
PARAMETER_READWRITE, (void*)&minimum_airspeed, sizeof(minimum_airspeed) },
+ {"ASPD_MAX", {0}, {300.0}, UDB_TYPE_M_AIRSPEED_TO_CM,
PARAMETER_READWRITE, (void*)&maximum_airspeed, sizeof(maximum_airspeed) },
+ {"ASPD_CRUISE", {0}, {300.0}, UDB_TYPE_M_AIRSPEED_TO_CM,
PARAMETER_READWRITE, (void*)&cruise_airspeed, sizeof(cruise_airspeed) },
{"ASPD_P_MIN_ASPD", {-90}, {90.0}, UDB_TYPE_DCM_ANGLE,
PARAMETER_READWRITE, (void*)&airspeed_pitch_min_aspd,
sizeof(airspeed_pitch_min_aspd) },
{"ASPD_P_MAX_ASPD", {-90}, {90.0}, UDB_TYPE_DCM_ANGLE,
PARAMETER_READWRITE, (void*)&airspeed_pitch_max_aspd,
sizeof(airspeed_pitch_max_aspd) },
{"ASPD_P_RATE_LIM", {1.0}, {720.0}, UDB_TYPE_FRAME_ANGLERATE,
PARAMETER_READWRITE, (void*)&airspeed_pitch_adjust_rate,
sizeof(airspeed_pitch_adjust_rate) },
{"ASPD_P_KI", {0.0}, {1.0}, UDB_TYPE_Q14, PARAMETER_READWRITE,
(void*)&airspeed_pitch_ki, sizeof(airspeed_pitch_ki) },
{"ASPD_P_KI_LIMIT", {0.0}, {45.0}, UDB_TYPE_DCM_ANGLE,
PARAMETER_READWRITE, (void*)&airspeed_pitch_ki_limit,
sizeof(airspeed_pitch_ki_limit) },

+ {"TURN_ELE_TR_NRM", {-1.0}, {1.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.ElevatorTrimNormal, sizeof(turns.ElevatorTrimNormal) },
+ {"TURN_ELE_TR_INV", {-1.0}, {1.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.ElevatorTrimInverted, sizeof(turns.ElevatorTrimInverted) },
+ {"TURN_CRUISE_SPD", {0.0}, {999.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.CruiseSpeed, sizeof(turns.CruiseSpeed) },
+ {"TURN_AOA_NORMAL", {-90.0}, {90.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.AngleOfAttackNormal, sizeof(turns.AngleOfAttackNormal) },
+ {"TURN_AOA_INV", {-90.0}, {90.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.AngleOfAttackInverted, sizeof(turns.AngleOfAttackInverted) },
+ {"TURN_FEED_FWD", {0.0}, {100.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.FeedForward, sizeof(turns.FeedForward) },
+ {"TURN_RATE_NAV", {0.0}, {100.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.TurnRateNav, sizeof(turns.TurnRateNav) },
+ {"TURN_RATE_FBW", {0.0}, {100.0}, UDB_TYPE_FLOAT, PARAMETER_READWRITE,
(void*)&turns.TurnRateFBW, sizeof(turns.TurnRateFBW) },
+
};

const uint16_t count_of_parameters_list = sizeof(mavlink_parameters_list)
/ sizeof(mavlink_parameter);
=======================================
--- /trunk/MatrixPilot/parameter_table_init.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/parameter_table_init.c Sat Aug 1 04:50:37 2015 UTC
@@ -20,68 +20,76 @@
mavlink_parameters_list[3].min.param_float=0.0;
mavlink_parameters_list[3].max.param_float=0.5; // yawkdail - PID_YAWKDAIL
mavlink_parameters_list[4].min.param_float=0.0;
mavlink_parameters_list[4].max.param_float=0.5; // pitchgain - PID_PITCHGAIN
mavlink_parameters_list[5].min.param_float=0.0;
mavlink_parameters_list[5].max.param_float=0.5; // pitchkd - PID_PITCHKD
- mavlink_parameters_list[6].min.param_float=0.0;
mavlink_parameters_list[6].max.param_float=0.5; // rudderElevMixGain -
PID_RUDELEVGAIN
- mavlink_parameters_list[7].min.param_float=0.0;
mavlink_parameters_list[7].max.param_float=0.5; // rollkprud - PID_ROLLKPRUD
- mavlink_parameters_list[8].min.param_float=0.0;
mavlink_parameters_list[8].max.param_float=0.5; // yawkprud - PID_YAWKPRUD
- mavlink_parameters_list[9].min.param_float=0.0;
mavlink_parameters_list[9].max.param_float=0.5; // yawkdrud - PID_YAWKDRUD
- mavlink_parameters_list[10].min.param_float=0.0;
mavlink_parameters_list[10].max.param_float=0.5; // rollkprud -
PID_ROLLKPRUD
- mavlink_parameters_list[11].min.param_float=0.0;
mavlink_parameters_list[11].max.param_float=0.5; // rollkdrud -
PID_ROLLKDRUD
+ mavlink_parameters_list[6].min.param_float=0.0;
mavlink_parameters_list[6].max.param_float=0.5; // rollkprud - PID_ROLLKPRUD
+ mavlink_parameters_list[7].min.param_float=0.0;
mavlink_parameters_list[7].max.param_float=0.5; // yawkprud - PID_YAWKPRUD
+ mavlink_parameters_list[8].min.param_float=0.0;
mavlink_parameters_list[8].max.param_float=0.5; // yawkdrud - PID_YAWKDRUD
+ mavlink_parameters_list[9].min.param_float=0.0;
mavlink_parameters_list[9].max.param_float=0.5; // rollkprud - PID_ROLLKPRUD
+ mavlink_parameters_list[10].min.param_float=0.0;
mavlink_parameters_list[10].max.param_float=0.5; // rollkdrud -
PID_ROLLKDRUD

- mavlink_parameters_list[12].min.param_int32=-32767;
mavlink_parameters_list[12].max.param_int32=32767; // rawMagCalib[0] -
MAG_CAL_RAW0
- mavlink_parameters_list[13].min.param_int32=-32767;
mavlink_parameters_list[13].max.param_int32=32767; // rawMagCalib[1] -
MAG_CAL_RAW1
- mavlink_parameters_list[14].min.param_int32=-32767;
mavlink_parameters_list[14].max.param_int32=32767; // rawMagCalib[2] -
MAG_CAL_RAW2
- mavlink_parameters_list[15].min.param_int32=-32767;
mavlink_parameters_list[15].max.param_int32=32767; // magGain[0] - MAG_GAIN0
- mavlink_parameters_list[16].min.param_int32=-32767;
mavlink_parameters_list[16].max.param_int32=32767; // magGain[1] - MAG_GAIN1
- mavlink_parameters_list[17].min.param_int32=-32767;
mavlink_parameters_list[17].max.param_int32=32767; // magGain[2] - MAG_GAIN2
- mavlink_parameters_list[18].min.param_int32=-32767;
mavlink_parameters_list[18].max.param_int32=32767; // udb_magOffset[0] -
MAG_OFFSET0
- mavlink_parameters_list[19].min.param_int32=-32767;
mavlink_parameters_list[19].max.param_int32=32767; // udb_magOffset[1] -
MAG_OFFSET1
- mavlink_parameters_list[20].min.param_int32=-32767;
mavlink_parameters_list[20].max.param_int32=32767; // udb_magOffset[2] -
MAG_OFFSET2
- mavlink_parameters_list[21].min.param_int32=-180;
mavlink_parameters_list[21].max.param_int32=180; //
dcm_declination_angle.BB - MAG_DECLINATION
+ mavlink_parameters_list[11].min.param_int32=-32767;
mavlink_parameters_list[11].max.param_int32=32767; // rawMagCalib[0] -
MAG_CAL_RAW0
+ mavlink_parameters_list[12].min.param_int32=-32767;
mavlink_parameters_list[12].max.param_int32=32767; // rawMagCalib[1] -
MAG_CAL_RAW1
+ mavlink_parameters_list[13].min.param_int32=-32767;
mavlink_parameters_list[13].max.param_int32=32767; // rawMagCalib[2] -
MAG_CAL_RAW2
+ mavlink_parameters_list[14].min.param_int32=-32767;
mavlink_parameters_list[14].max.param_int32=32767; // magGain[0] - MAG_GAIN0
+ mavlink_parameters_list[15].min.param_int32=-32767;
mavlink_parameters_list[15].max.param_int32=32767; // magGain[1] - MAG_GAIN1
+ mavlink_parameters_list[16].min.param_int32=-32767;
mavlink_parameters_list[16].max.param_int32=32767; // magGain[2] - MAG_GAIN2
+ mavlink_parameters_list[17].min.param_int32=-32767;
mavlink_parameters_list[17].max.param_int32=32767; // udb_magOffset[0] -
MAG_OFFSET0
+ mavlink_parameters_list[18].min.param_int32=-32767;
mavlink_parameters_list[18].max.param_int32=32767; // udb_magOffset[1] -
MAG_OFFSET1
+ mavlink_parameters_list[19].min.param_int32=-32767;
mavlink_parameters_list[19].max.param_int32=32767; // udb_magOffset[2] -
MAG_OFFSET2
+ mavlink_parameters_list[20].min.param_int32=-180;
mavlink_parameters_list[20].max.param_int32=180; //
dcm_declination_angle.BB - MAG_DECLINATION

- mavlink_parameters_list[22].min.param_float=800.0;
mavlink_parameters_list[22].max.param_float=2200.0; //
udb_pwTrim[AILERON_INPUT_CHANNEL] - PWTRIM_AILERON
- mavlink_parameters_list[23].min.param_float=800.0;
mavlink_parameters_list[23].max.param_float=2200.0; //
udb_pwTrim[ELEVATOR_INPUT_CHANNEL] - PWTRIM_ELEVATOR
- mavlink_parameters_list[24].min.param_float=800.0;
mavlink_parameters_list[24].max.param_float=2200.0; //
udb_pwTrim[RUDDER_INPUT_CHANNEL] - PWTRIM_RUDDER
- mavlink_parameters_list[25].min.param_float=800.0;
mavlink_parameters_list[25].max.param_float=2200.0; //
udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL] - PWTRIM_AILERON2
- mavlink_parameters_list[26].min.param_float=800.0;
mavlink_parameters_list[26].max.param_float=2200.0; //
udb_pwTrim[ROLL_INPUT_CHANNEL] - PWTRIM_ROLL
- mavlink_parameters_list[27].min.param_float=800.0;
mavlink_parameters_list[27].max.param_float=2200.0; //
udb_pwTrim[PITCH_INPUT_CHANNEL] - PWTRIM_PITCH
- mavlink_parameters_list[28].min.param_float=800.0;
mavlink_parameters_list[28].max.param_float=2200.0; //
udb_pwTrim[THROTTLE_INPUT_CHANNEL] - PWTRIM_THROTTLE
- mavlink_parameters_list[29].min.param_float=800.0;
mavlink_parameters_list[29].max.param_float=2200.0; //
udb_pwTrim[YAW_INPUT_CHANNEL] - PWTRIM_YAW
- mavlink_parameters_list[30].min.param_float=800.0;
mavlink_parameters_list[30].max.param_float=2200.0; //
udb_pwTrim[FLAP_INPUT_CHANNEL] - PWTRIM_FLAP
- mavlink_parameters_list[31].min.param_float=800.0;
mavlink_parameters_list[31].max.param_float=2200.0; //
udb_pwTrim[BRAKE_INPUT_CHANNEL] - PWTRIM_AIRBRAKE
- mavlink_parameters_list[32].min.param_float=800.0;
mavlink_parameters_list[32].max.param_float=2200.0; //
udb_pwTrim[SPOILER_INPUT_CHANNEL] - PWTRIM_SPOILER
- mavlink_parameters_list[33].min.param_float=800.0;
mavlink_parameters_list[33].max.param_float=2200.0; //
udb_pwTrim[CAMBER_INPUT_CHANNEL] - PWTRIM_CAMBER
- mavlink_parameters_list[34].min.param_float=800.0;
mavlink_parameters_list[34].max.param_float=2200.0; //
udb_pwTrim[CROW_INPUT_CHANNEL] - PWTRIM_CROW
- mavlink_parameters_list[35].min.param_float=800.0;
mavlink_parameters_list[35].max.param_float=2200.0; //
udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL] - PWTRIM_CAMPITCH
- mavlink_parameters_list[36].min.param_float=800.0;
mavlink_parameters_list[36].max.param_float=2200.0; //
udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL] - PWTRIM_CAM_YAW
+ mavlink_parameters_list[21].min.param_float=800.0;
mavlink_parameters_list[21].max.param_float=2200.0; //
udb_pwTrim[AILERON_INPUT_CHANNEL] - PWTRIM_AILERON
+ mavlink_parameters_list[22].min.param_float=800.0;
mavlink_parameters_list[22].max.param_float=2200.0; //
udb_pwTrim[ELEVATOR_INPUT_CHANNEL] - PWTRIM_ELEVATOR
+ mavlink_parameters_list[23].min.param_float=800.0;
mavlink_parameters_list[23].max.param_float=2200.0; //
udb_pwTrim[RUDDER_INPUT_CHANNEL] - PWTRIM_RUDDER
+ mavlink_parameters_list[24].min.param_float=800.0;
mavlink_parameters_list[24].max.param_float=2200.0; //
udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL] - PWTRIM_AILERON2
+ mavlink_parameters_list[25].min.param_float=800.0;
mavlink_parameters_list[25].max.param_float=2200.0; //
udb_pwTrim[ROLL_INPUT_CHANNEL] - PWTRIM_ROLL
+ mavlink_parameters_list[26].min.param_float=800.0;
mavlink_parameters_list[26].max.param_float=2200.0; //
udb_pwTrim[PITCH_INPUT_CHANNEL] - PWTRIM_PITCH
+ mavlink_parameters_list[27].min.param_float=800.0;
mavlink_parameters_list[27].max.param_float=2200.0; //
udb_pwTrim[THROTTLE_INPUT_CHANNEL] - PWTRIM_THROTTLE
+ mavlink_parameters_list[28].min.param_float=800.0;
mavlink_parameters_list[28].max.param_float=2200.0; //
udb_pwTrim[YAW_INPUT_CHANNEL] - PWTRIM_YAW
+ mavlink_parameters_list[29].min.param_float=800.0;
mavlink_parameters_list[29].max.param_float=2200.0; //
udb_pwTrim[FLAP_INPUT_CHANNEL] - PWTRIM_FLAP
+ mavlink_parameters_list[30].min.param_float=800.0;
mavlink_parameters_list[30].max.param_float=2200.0; //
udb_pwTrim[BRAKE_INPUT_CHANNEL] - PWTRIM_AIRBRAKE
+ mavlink_parameters_list[31].min.param_float=800.0;
mavlink_parameters_list[31].max.param_float=2200.0; //
udb_pwTrim[SPOILER_INPUT_CHANNEL] - PWTRIM_SPOILER
+ mavlink_parameters_list[32].min.param_float=800.0;
mavlink_parameters_list[32].max.param_float=2200.0; //
udb_pwTrim[CAMBER_INPUT_CHANNEL] - PWTRIM_CAMBER
+ mavlink_parameters_list[33].min.param_float=800.0;
mavlink_parameters_list[33].max.param_float=2200.0; //
udb_pwTrim[CROW_INPUT_CHANNEL] - PWTRIM_CROW
+ mavlink_parameters_list[34].min.param_float=800.0;
mavlink_parameters_list[34].max.param_float=2200.0; //
udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL] - PWTRIM_CAMPITCH
+ mavlink_parameters_list[35].min.param_float=800.0;
mavlink_parameters_list[35].max.param_float=2200.0; //
udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL] - PWTRIM_CAM_YAW

- mavlink_parameters_list[37].min.param_int32=-32767;
mavlink_parameters_list[37].max.param_int32=32767; // udb_xaccel.offset -
IMU_XACCEL_OFF
- mavlink_parameters_list[38].min.param_int32=-32767;
mavlink_parameters_list[38].max.param_int32=32767; // udb_yaccel.offset -
IMU_YACCEL_OFF
- mavlink_parameters_list[39].min.param_int32=-32767;
mavlink_parameters_list[39].max.param_int32=32767; // udb_zaccel.offset -
IMU_ZACCEL_OFF
- mavlink_parameters_list[40].min.param_int32=-32767;
mavlink_parameters_list[40].max.param_int32=32767; // udb_xrate.offset -
IMU_XGYRO_OFF
- mavlink_parameters_list[41].min.param_int32=-32767;
mavlink_parameters_list[41].max.param_int32=32767; // udb_yrate.offset -
IMU_YGYRO_OFF
- mavlink_parameters_list[42].min.param_int32=-32767;
mavlink_parameters_list[42].max.param_int32=32767; // udb_zrate.offset -
IMU_ZGYRO_OFF
- mavlink_parameters_list[43].min.param_int32=-32767;
mavlink_parameters_list[43].max.param_int32=32767; // udb_vref.offset -
IMU_VREF_OFF
+ mavlink_parameters_list[36].min.param_int32=-32767;
mavlink_parameters_list[36].max.param_int32=32767; // udb_xaccel.offset -
IMU_XACCEL_OFF
+ mavlink_parameters_list[37].min.param_int32=-32767;
mavlink_parameters_list[37].max.param_int32=32767; // udb_yaccel.offset -
IMU_YACCEL_OFF
+ mavlink_parameters_list[38].min.param_int32=-32767;
mavlink_parameters_list[38].max.param_int32=32767; // udb_zaccel.offset -
IMU_ZACCEL_OFF
+ mavlink_parameters_list[39].min.param_int32=-32767;
mavlink_parameters_list[39].max.param_int32=32767; // udb_xrate.offset -
IMU_XGYRO_OFF
+ mavlink_parameters_list[40].min.param_int32=-32767;
mavlink_parameters_list[40].max.param_int32=32767; // udb_yrate.offset -
IMU_YGYRO_OFF
+ mavlink_parameters_list[41].min.param_int32=-32767;
mavlink_parameters_list[41].max.param_int32=32767; // udb_zrate.offset -
IMU_ZGYRO_OFF
+ mavlink_parameters_list[42].min.param_int32=-32767;
mavlink_parameters_list[42].max.param_int32=32767; // udb_vref.offset -
IMU_VREF_OFF
+
+ mavlink_parameters_list[43].min.param_int32=0;
mavlink_parameters_list[43].max.param_int32=5000; // height_target_min -
TH_H_TARGET_MIN
+ mavlink_parameters_list[44].min.param_int32=0;
mavlink_parameters_list[44].max.param_int32=5000; // height_target_max -
TH_H_TARGET_MAX
+ mavlink_parameters_list[45].min.param_int32=1;
mavlink_parameters_list[45].max.param_int32=500; // height_margin -
TH_H_MARGIN
+ mavlink_parameters_list[46].min.param_float=0;
mavlink_parameters_list[46].max.param_float=1; // alt_hold_throttle_min -
TH_T_HOLD_MIN
+ mavlink_parameters_list[47].min.param_float=0;
mavlink_parameters_list[47].max.param_float=1; // alt_hold_throttle_max -
TH_T_HOLD_MAX
+ mavlink_parameters_list[48].min.param_int32=-89;
mavlink_parameters_list[48].max.param_int32=0; // alt_hold_pitch_min -
TH_P_HOLD_MIN
+ mavlink_parameters_list[49].min.param_int32=0;
mavlink_parameters_list[49].max.param_int32=89; // alt_hold_pitch_max -
TH_P_HOLD_MAX
+ mavlink_parameters_list[50].min.param_int32=0;
mavlink_parameters_list[50].max.param_int32=89; // alt_hold_pitch_high -
TH_P_HIGH
+ mavlink_parameters_list[51].min.param_int32=0;
mavlink_parameters_list[51].max.param_int32=89; // rtl_pitch_down -
TH_P_RTL_DOWN

- mavlink_parameters_list[44].min.param_int32=0;
mavlink_parameters_list[44].max.param_int32=5000; // height_target_min -
TH_H_TARGET_MIN
- mavlink_parameters_list[45].min.param_int32=0;
mavlink_parameters_list[45].max.param_int32=5000; // height_target_max -
TH_H_TARGET_MAX
- mavlink_parameters_list[46].min.param_int32=1;
mavlink_parameters_list[46].max.param_int32=500; // height_margin -
TH_H_MARGIN
- mavlink_parameters_list[47].min.param_float=0;
mavlink_parameters_list[47].max.param_float=1; // alt_hold_throttle_min -
TH_T_HOLD_MIN
- mavlink_parameters_list[48].min.param_float=0;
mavlink_parameters_list[48].max.param_float=1; // alt_hold_throttle_max -
TH_T_HOLD_MAX
- mavlink_parameters_list[49].min.param_int32=-89;
mavlink_parameters_list[49].max.param_int32=0; // alt_hold_pitch_min -
TH_P_HOLD_MIN
- mavlink_parameters_list[50].min.param_int32=0;
mavlink_parameters_list[50].max.param_int32=89; // alt_hold_pitch_max -
TH_P_HOLD_MAX
- mavlink_parameters_list[51].min.param_int32=0;
mavlink_parameters_list[51].max.param_int32=89; // alt_hold_pitch_high -
TH_P_HIGH
- mavlink_parameters_list[52].min.param_int32=0;
mavlink_parameters_list[52].max.param_int32=89; // rtl_pitch_down -
TH_P_RTL_DOWN
+ mavlink_parameters_list[52].min.param_float=0;
mavlink_parameters_list[52].max.param_float=300.0; // desiredSpeed -
ASPD_DESIRED
+ mavlink_parameters_list[53].min.param_float=0;
mavlink_parameters_list[53].max.param_float=20000; // minimum_groundspeed -
ASPD_MIN_GSPD
+ mavlink_parameters_list[54].min.param_float=0;
mavlink_parameters_list[54].max.param_float=300.0; // minimum_airspeed -
ASPD_MIN
+ mavlink_parameters_list[55].min.param_float=0;
mavlink_parameters_list[55].max.param_float=300.0; // maximum_airspeed -
ASPD_MAX
+ mavlink_parameters_list[56].min.param_float=0;
mavlink_parameters_list[56].max.param_float=300.0; // cruise_airspeed -
ASPD_CRUISE
+ mavlink_parameters_list[57].min.param_int32=-90;
mavlink_parameters_list[57].max.param_int32=90.0; //
airspeed_pitch_min_aspd - ASPD_P_MIN_ASPD
+ mavlink_parameters_list[58].min.param_int32=-90;
mavlink_parameters_list[58].max.param_int32=90.0; //
airspeed_pitch_max_aspd - ASPD_P_MAX_ASPD
+ mavlink_parameters_list[59].min.param_int32=1.0;
mavlink_parameters_list[59].max.param_int32=720.0; //
airspeed_pitch_adjust_rate - ASPD_P_RATE_LIM
+ mavlink_parameters_list[60].min.param_float=0.0;
mavlink_parameters_list[60].max.param_float=1.0; // airspeed_pitch_ki -
ASPD_P_KI
+ mavlink_parameters_list[61].min.param_int32=0.0;
mavlink_parameters_list[61].max.param_int32=45.0; //
airspeed_pitch_ki_limit - ASPD_P_KI_LIMIT

- mavlink_parameters_list[53].min.param_float=0;
mavlink_parameters_list[53].max.param_float=300.0; // desiredSpeed -
ASPD_DESIRED
- mavlink_parameters_list[54].min.param_float=0;
mavlink_parameters_list[54].max.param_float=20000; // minimum_groundspeed -
ASPD_MIN_GSPD
- mavlink_parameters_list[55].min.param_float=0;
mavlink_parameters_list[55].max.param_float=300.0; // minimum_airspeed -
ASPD_MIN
- mavlink_parameters_list[56].min.param_float=0;
mavlink_parameters_list[56].max.param_float=300.0; // maximum_airspeed -
ASPD_MAX
- mavlink_parameters_list[57].min.param_float=0;
mavlink_parameters_list[57].max.param_float=300.0; // cruise_airspeed -
ASPD_CRUISE
- mavlink_parameters_list[58].min.param_int32=-90;
mavlink_parameters_list[58].max.param_int32=90.0; //
airspeed_pitch_min_aspd - ASPD_P_MIN_ASPD
- mavlink_parameters_list[59].min.param_int32=-90;
mavlink_parameters_list[59].max.param_int32=90.0; //
airspeed_pitch_max_aspd - ASPD_P_MAX_ASPD
- mavlink_parameters_list[60].min.param_int32=1.0;
mavlink_parameters_list[60].max.param_int32=720.0; //
airspeed_pitch_adjust_rate - ASPD_P_RATE_LIM
- mavlink_parameters_list[61].min.param_float=0.0;
mavlink_parameters_list[61].max.param_float=1.0; // airspeed_pitch_ki -
ASPD_P_KI
- mavlink_parameters_list[62].min.param_int32=0.0;
mavlink_parameters_list[62].max.param_int32=45.0; //
airspeed_pitch_ki_limit - ASPD_P_KI_LIMIT
+ mavlink_parameters_list[62].min.param_float=-1.0;
mavlink_parameters_list[62].max.param_float=1.0; //
turns.ElevatorTrimNormal - TURN_ELE_TR_NRM
+ mavlink_parameters_list[63].min.param_float=-1.0;
mavlink_parameters_list[63].max.param_float=1.0; //
turns.ElevatorTrimInverted - TURN_ELE_TR_INV
+ mavlink_parameters_list[64].min.param_float=0.0;
mavlink_parameters_list[64].max.param_float=999.0; // turns.CruiseSpeed -
TURN_CRUISE_SPD
+ mavlink_parameters_list[65].min.param_float=-90.0;
mavlink_parameters_list[65].max.param_float=90.0; //
turns.AngleOfAttackNormal - TURN_AOA_NORMAL
+ mavlink_parameters_list[66].min.param_float=-90.0;
mavlink_parameters_list[66].max.param_float=90.0; //
turns.AngleOfAttackInverted - TURN_AOA_INV
+ mavlink_parameters_list[67].min.param_float=0.0;
mavlink_parameters_list[67].max.param_float=100.0; // turns.FeedForward -
TURN_FEED_FWD
+ mavlink_parameters_list[68].min.param_float=0.0;
mavlink_parameters_list[68].max.param_float=100.0; // turns.TurnRateNav -
TURN_RATE_NAV
+ mavlink_parameters_list[69].min.param_float=0.0;
mavlink_parameters_list[69].max.param_float=100.0; // turns.TurnRateFBW -
TURN_RATE_FBW

};

=======================================
--- /trunk/MatrixPilot/pitchCntrl.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/pitchCntrl.c Sat Aug 1 04:50:37 2015 UTC
@@ -31,36 +31,31 @@
#include "../libUDB/servoOut.h"
#include "../libDCM/rmat.h"

-// If the state machine selects pitch feedback, compute it from the pitch
gyro and accelerometer.
-
-#define ANGLE_90DEG (RMAX/(2*57.3)) // FIXME: never used
-#define RTLKICK ((int32_t)(RTL_PITCH_DOWN*(RMAX/57.3)))
-#define INVNPITCH ((int32_t)(INVERTED_NEUTRAL_PITCH*(RMAX/57.3)))
#define HOVERPOFFSET ((int32_t)(hover.HoverPitchOffset*(RMAX/57.3)))
#define HOVERPTOWP ((int32_t)(hover.HoverPitchTowardsWP*(RMAX/57.3)))

uint16_t pitchgain;
+uint16_t pitchfdfwd;
uint16_t pitchkd;
uint16_t hoverpitchgain;
uint16_t hoverpitchkd;
-uint16_t rudderElevMixGain;
-uint16_t rollElevMixGain;

int16_t pitchrate;
-int16_t navElevMix;
+
int16_t elevInput;

+int16_t elevatorLoadingTrim = 0 ;
+
static void normalPitchCntrl(void);
static void hoverPitchCntrl(void);

void init_pitchCntrl(void)
{
pitchgain = (uint16_t)(gains.Pitchgain*RMAX);
+ pitchfdfwd = (uint16_t)(turns.FeedForward*gains.Pitchgain*RMAX);
pitchkd = (uint16_t) (gains.PitchKD*SCALEGYRO*RMAX);
hoverpitchgain = (uint16_t)(hover.HoverPitchGain*RMAX);
hoverpitchkd = (uint16_t) (hover.HoverPitchKD*SCALEGYRO*RMAX);
- rudderElevMixGain = (uint16_t)(RUDDER_ELEV_MIX*RMAX);
- rollElevMixGain = (uint16_t)(ROLL_ELEV_MIX*RMAX);
}

void save_pitchCntrl(void)
@@ -69,8 +64,8 @@
gains.PitchKD = (float)pitchkd / (SCALEGYRO*RMAX);
hover.HoverPitchGain = (float)hoverpitchgain / (RMAX);
hover.HoverPitchKD = (float)hoverpitchkd / (SCALEGYRO*RMAX);
- gains.RudderElevMix = (float)rudderElevMixGain / (RMAX);
- gains.RollElevMix = (float)rollElevMixGain / (RMAX);
+// gains.RudderElevMix = (float)rudderElevMixGain / (RMAX);
+// gains.RollElevMix = (float)rollElevMixGain / (RMAX);
}

void pitchCntrl(void)
@@ -88,72 +83,25 @@
static void normalPitchCntrl(void)
{
union longww pitchAccum;
- int16_t rtlkick;
// int16_t aspd_adj;
// fractional aspd_err, aspd_diff;
- fractional rmat6;
- fractional rmat7;
- fractional rmat8;

#ifdef TestGains
state_flags._.GPS_steering = 0; // turn navigation off
state_flags._.pitch_feedback = 1; // turn stabilization on
#endif
- if (!canStabilizeInverted() || current_orientation != F_INVERTED)
+
+ if (settings._.PitchStabilization && state_flags._.pitch_feedback)
{
- rmat6 = rmat[6];
- rmat7 = rmat[7];
- rmat8 = rmat[8];
+ pitchAccum.WW = __builtin_mulsu(tiltError[0], pitchgain)
+ - __builtin_mulsu(desiredRotationRateRadians[0],
pitchfdfwd)
+ + __builtin_mulsu(rotationRateError[0], pitchkd );
+ pitch_control = (int32_t)pitchAccum._.W1 + (int32_t) elevatorLoadingTrim;
}
else
{
- rmat6 = -rmat[6];
- rmat7 = -rmat[7];
- rmat8 = -rmat[8];
- pitchAltitudeAdjust = -pitchAltitudeAdjust - INVNPITCH;
+ pitch_control = 0;
}
- navElevMix = 0;
- if (state_flags._.pitch_feedback)
- {
- if (RUDDER_OUTPUT_CHANNEL != CHANNEL_UNUSED && RUDDER_INPUT_CHANNEL !=
CHANNEL_UNUSED) {
- pitchAccum.WW = __builtin_mulsu(rmat6, rudderElevMixGain) << 1;
- pitchAccum.WW = __builtin_mulss(pitchAccum._.W1,
- REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED,
udb_pwTrim[RUDDER_INPUT_CHANNEL] - udb_pwOut[RUDDER_OUTPUT_CHANNEL])) << 3;
- navElevMix += pitchAccum._.W1;
- }
- pitchAccum.WW = __builtin_mulsu(rmat6, rollElevMixGain) << 1;
- pitchAccum.WW = __builtin_mulss(pitchAccum._.W1, rmat[6]) >> 3;
- navElevMix += pitchAccum._.W1;
- }
- pitchAccum.WW = (__builtin_mulss(rmat8, omegagyro[0])
- - __builtin_mulss(rmat6, omegagyro[2])) << 1;
- pitchrate = pitchAccum._.W1;
- if (!udb_flags._.radio_on && state_flags._.GPS_steering)
- {
- rtlkick = RTLKICK;
- }
- else
- {
- rtlkick = 0;
- }
-#if (GLIDE_AIRSPEED_CONTROL == 1)
- fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
-#endif
- if (PITCH_STABILIZATION && state_flags._.pitch_feedback)
- {
-#if (GLIDE_AIRSPEED_CONTROL == 1)
- pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + aspd_pitch_adj +
pitchAltitudeAdjust, pitchgain)
- + __builtin_mulus(pitchkd, pitchrate);
-#else
- pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + pitchAltitudeAdjust,
pitchgain)
- + __builtin_mulus(pitchkd, pitchrate);
-#endif
- }
- else
- {
- pitchAccum.WW = 0;
- }
- pitch_control = (int32_t)pitchAccum._.W1 + navElevMix;
}

static void hoverPitchCntrl(void)
=======================================
--- /trunk/MatrixPilot/ring_buffer.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/ring_buffer.c Sat Aug 1 04:50:37 2015 UTC
@@ -29,8 +29,6 @@

#ifdef USE_RING_BUFFER

-void udb_serial_start_sending_data(void);
-
#include <xc.h>
#include <stdint.h>
#include <string.h>
=======================================
--- /trunk/MatrixPilot/rollCntrl.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/rollCntrl.c Sat Aug 1 04:50:37 2015 UTC
@@ -30,6 +30,7 @@

uint16_t yawkdail;
uint16_t rollkp;
+uint16_t rollkpfdfwd;
uint16_t rollkd;
uint16_t hoverrollkp;
uint16_t hoverrollkd;
@@ -41,6 +42,7 @@
{
yawkdail = (uint16_t)(gains.YawKDAileron*SCALEGYRO*RMAX);
rollkp = (uint16_t)(gains.RollKP*RMAX);
+ rollkpfdfwd = (uint16_t)(turns.FeedForward*gains.RollKP*RMAX);
rollkd = (uint16_t)(gains.RollKD*SCALEGYRO*RMAX);
hoverrollkp = (uint16_t)(hover.HoverRollKP*SCALEGYRO*RMAX);
hoverrollkd = (uint16_t)(hover.HoverRollKD*SCALEGYRO*RMAX);
@@ -72,33 +74,25 @@
union longww rollAccum = { 0 };
union longww gyroRollFeedback;
union longww gyroYawFeedback;
- fractional rmat6;
fractional omegaAccum2;

if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
- rmat6 = rmat[6];
omegaAccum2 = omegaAccum[2];
}
else
{
- rmat6 = -rmat[6];
omegaAccum2 = -omegaAccum[2];
}
-#ifdef TestGains
- state_flags._.GPS_steering = 0; // turn off navigation
-#endif
- if (AILERON_NAVIGATION && state_flags._.GPS_steering)
- {
- rollAccum._.W1 = navigate_determine_deflection('a');
- }
+
#ifdef TestGains
state_flags._.pitch_feedback = 1;
#endif
if (settings._.RollStabilizaionAilerons && state_flags._.pitch_feedback)
{
- gyroRollFeedback.WW = __builtin_mulus(rollkd, omegaAccum[1]);
- rollAccum.WW += __builtin_mulsu(rmat6, rollkp);
+ gyroRollFeedback.WW = - __builtin_mulus(rollkd, rotationRateError[1]);
+ rollAccum.WW -= __builtin_mulsu(tiltError[1], rollkp);
+ rollAccum.WW += __builtin_mulsu(desiredRotationRateRadians[1],
rollkpfdfwd);
}
else
{
@@ -106,13 +100,13 @@
}
if (settings._.YawStabilizationAileron && state_flags._.pitch_feedback)
{
- gyroYawFeedback.WW = __builtin_mulus(yawkdail, omegaAccum2);
+ gyroYawFeedback.WW = - __builtin_mulus(yawkdail, omegaAccum2);
}
else
{
gyroYawFeedback.WW = 0;
}
- roll_control = (int32_t)rollAccum._.W1 - (int32_t)gyroRollFeedback._.W1 -
(int32_t)gyroYawFeedback._.W1;
+ roll_control = (int32_t)rollAccum._.W1 + (int32_t)gyroRollFeedback._.W1 +
(int32_t)gyroYawFeedback._.W1;
// Servo reversing is handled in servoMix.c
}

=======================================
--- /trunk/MatrixPilot/servoMix.c Mon Jun 15 09:42:26 2015 UTC
+++ /trunk/MatrixPilot/servoMix.c Sat Aug 1 04:50:37 2015 UTC
@@ -33,17 +33,11 @@
//
// Mix computed roll and pitch controls into the output channels for the
compiled airframe type.

-//const int16_t aileronbgain = (int16_t)(8.0*AILERON_BOOST);
-//const int16_t elevatorbgain = (int16_t)(8.0*ELEVATOR_BOOST);
-//const int16_t rudderbgain = (int16_t)(8.0*RUDDER_BOOST);
-
-static int16_t aileronbgain = 0;
static int16_t elevatorbgain = 0;
static int16_t rudderbgain = 0;

void servoMix_init(void)
{
- aileronbgain = (int16_t)(8.0*gains.AileronBoost);
elevatorbgain = (int16_t)(8.0*gains.ElevatorBoost);
rudderbgain = (int16_t)(8.0*gains.RudderBoost);
}
@@ -62,24 +56,25 @@
pwManual[temp] = udb_pwTrim[temp];
}

- // Apply boosts if in a stabilized mode
- if (udb_flags._.radio_on && state_flags._.pitch_feedback)
- {
- pwManual[AILERON_INPUT_CHANNEL] += ((pwManual[AILERON_INPUT_CHANNEL] -
udb_pwTrim[AILERON_INPUT_CHANNEL]) * aileronbgain) >> 3;
- pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] -
udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3;
- pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
- }

// Standard airplane airframe
// Mix roll_control into ailerons
// Mix pitch_control into elevators
// Mix yaw control and waggle into rudder
#if (AIRFRAME_TYPE == AIRFRAME_STANDARD)
+ // Apply boosts to elevator and rudder if in a controlled mode
+ // It does not matter whether the radio is on or not
+ if (state_flags._.pitch_feedback)
+ {
+ pwManual[AILERON_INPUT_CHANNEL] = udb_pwTrim[AILERON_INPUT_CHANNEL] ; //
in fly by wire or navigate mode, manual input is accounted for in the turn
control
+ pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] -
udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3;
+ pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
+ }
temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
- REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - 3000);
+ udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] =
udb_pwTrim[AILERON_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]);

temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
@@ -96,29 +91,57 @@
temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
}
-#endif
+#endif // AIRFRAME_STANDARD

// V-Tail airplane airframe
// Mix roll_control and waggle into ailerons
// Mix pitch_control and yaw_control into both elevator and rudder
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
{
+ int16_t rudderInput;
+ int16_t elevatorInput;
+ int16_t pitchInput;
+ int16_t yawInput;
+ int16_t pitchCommand;
+ int16_t yawCommand;
int32_t vtail_yaw_control;
- vtail_yaw_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
yaw_control);
+
+ // Unmix the vtail
+ rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED,
(pwManual[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
+ elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED,
(pwManual[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
+ pitchInput = ((rudderInput+elevatorInput)>>1);
+ yawInput = ((-rudderInput+elevatorInput)>>1);
+
+ if (state_flags._.pitch_feedback)
+ {
+ // Apply boost in FBW or navigate mode
+ pitchCommand = ((elevatorbgain + 8) * pitchInput) >> 3;
+ yawCommand = ((rudderbgain + 8) * yawInput) >> 3;
+ // in fly by wire or navigate mode, manual input is accounted for in the
turn control
+ pwManual[AILERON_INPUT_CHANNEL] = udb_pwTrim[AILERON_INPUT_CHANNEL];
+ }
+ else
+ {
+ pitchCommand = pitchInput;
+ yawCommand = yawInput;
+ }
+
+ vtail_yaw_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
yaw_control);
+
+ // In fly by wire mode, ailerons are controlled indirectly by helical
turn control
+ temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
+ udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
- udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
-
- // Reverse the polarity of the secondary aileron if necessary
- udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
- REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - 3000);
+ // Reverse the polarity of the secondary aileron if necessary
+ udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] =
udb_pwTrim[AILERON_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]);

- temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control +
vtail_yaw_control);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = udb_pwTrim[ELEVATOR_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitchCommand +
pitch_control + yawCommand + vtail_yaw_control);
+ udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- temp = pwManual[RUDDER_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitch_control -
vtail_yaw_control);
+ temp = udb_pwTrim[RUDDER_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitchCommand +
pitch_control - yawCommand - vtail_yaw_control);
udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
@@ -137,18 +160,43 @@
// Mix roll_control, pitch_control, and waggle into aileron and elevator
// Mix rudder_control into rudder
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
- int32_t delta_roll_control =
REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, roll_control);
+ {
+ int16_t aileronInput;
+ int16_t elevatorInput;
+ int16_t pitchInput;
+ int16_t rollInput;
+ int16_t pitchCommand;
+ int16_t rollCommand;
+ int32_t delta_roll_control;
+ // unmix the inputs, note this will produce zeros during radio off
+ aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED,
(pwManual[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
+ elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED,
(pwManual[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
+ pitchInput = (elevatorInput+aileronInput)>>1;
+ rollInput = (elevatorInput-aileronInput)>>1;
+
+ if (state_flags._.pitch_feedback)
+ {
+ pitchCommand = ((elevatorbgain + 8) * pitchInput ) >> 3;
+ pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
+ rollCommand = 0;
+ }
+ else
+ {
+ pitchCommand = pitchInput;
+ rollCommand = rollInput;
+ }
+ delta_roll_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
roll_control);

- temp = pwManual[AILERON_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -delta_roll_control +
pitch_control - waggle);
+ temp = udb_pwTrim[AILERON_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -rollCommand
-delta_roll_control + pitchCommand + pitch_control - waggle);
udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, delta_roll_control +
pitch_control + waggle);
+ temp = udb_pwTrim[ELEVATOR_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, rollCommand +
delta_roll_control + pitchCommand + pitch_control + waggle);
udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

temp = pwManual[RUDDER_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control);
+ REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
@@ -160,6 +208,7 @@
temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
}
+ }
#endif // AIRFRAME_DELTA

// Helicopter airframe
=======================================
--- /trunk/MatrixPilot/servoPrepare.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/servoPrepare.h Sat Aug 1 04:50:37 2015 UTC
@@ -25,10 +25,15 @@

extern int16_t throttle_control;
extern int16_t pitch_control;
+extern int16_t elevatorLoadingTrim;
extern int16_t roll_control;
extern int16_t yaw_control;
extern uint16_t wind_gain;

+//extern int16_t tiltError[3];
+//extern int16_t rotationRateError[3];
+//extern int16_t desiredRotationRateRadians[3];
+
void servoPrepare_init(void);


=======================================
--- /trunk/MatrixPilot/telemetry.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/telemetry.c Sat Aug 1 04:50:37 2015 UTC
@@ -18,15 +18,17 @@
// You should have received a copy of the GNU General Public License
// along with MatrixPilot. If not, see <http://www.gnu.org/licenses/>.

+#include "defines.h"

-#include "defines.h"
+#define USE_TELEMETRY
+#ifdef USE_TELEMETRY
+
#include "states.h"
#include "config.h"
#include "navigate.h"
#include "cameraCntrl.h"
#include "flightplan.h"
#include "flightplan-waypoints.h"
-#include "telemetry.h"
#if (USE_TELELOG == 1)
#include "telemetry_log.h"
#endif
@@ -117,6 +119,9 @@
static int16_t sb_index = 0;
static int16_t end_index = 0;

+int16_t udb_serial_callback_get_byte_to_send(void);
+void udb_serial_callback_received_byte(uint8_t rxchar);
+
void telemetry_init(void)
{
#if (SERIAL_OUTPUT_FORMAT == SERIAL_OSD_REMZIBI)
@@ -874,3 +879,24 @@
{
}
#endif // SERIAL_OUTPUT_FORMAT
+
+#else // USE_TELEMETRY
+
+int16_t udb_serial_callback_get_byte_to_send(void)
+{
+ return -1;
+}
+void udb_serial_callback_received_byte(uint8_t rxchar)
+{
+}
+void telemetry_restart(void)
+{
+}
+void telemetry_output_8hz(void)
+{
+}
+void telemetry_init(void)
+{
+}
+
+#endif // USE_TELEMETRY
=======================================
--- /trunk/MatrixPilot/telemetry.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/telemetry.h Sat Aug 1 04:50:37 2015 UTC
@@ -25,4 +25,4 @@
void telemetry_output_8hz(void);

int16_t udb_serial_callback_get_byte_to_send(void);
-void udb_serial_callback_received_byte(uint8_t rxchar);
+//void udb_serial_callback_received_byte(uint8_t rxchar);
=======================================
--- /trunk/MatrixPilot/yawCntrl.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/MatrixPilot/yawCntrl.c Sat Aug 1 04:50:37 2015 UTC
@@ -28,9 +28,12 @@
#include "helicalTurnCntrl.h"
#include "../libDCM/rmat.h"

+#include "gain_variables.h"
+
#define HOVERYOFFSET ((int32_t)(hover.HoverYawOffset*(RMAX/57.3)))

uint16_t yawkdrud;
+uint16_t yawkpfdfwd;
uint16_t rollkprud;
uint16_t rollkdrud;
uint16_t hoveryawkp;
@@ -42,6 +45,7 @@
void init_yawCntrl(void)
{
yawkdrud = (uint16_t)(gains.YawKDRudder*SCALEGYRO*RMAX);
+ yawkpfdfwd = (uint16_t)(turns.FeedForward*gains.YawKPRudder*RMAX);
rollkprud = (uint16_t)(gains.RollKPRudder*RMAX);
rollkdrud = (uint16_t)(gains.RollKDRudder*SCALEGYRO*RMAX);
hoveryawkp = (uint16_t)(hover.HoverYawKP*RMAX);
@@ -71,9 +75,9 @@

void normalYawCntrl(void)
{
- int16_t yawNavDeflection;
union longww rollStabilization;
union longww gyroYawFeedback;
+ union longww yawStabilization;
int16_t ail_rud_mix;

#ifdef TestGains
@@ -83,39 +87,20 @@

if (settings._.YawStabilizationRudder && state_flags._.pitch_feedback)
{
- yawNavDeflection = navigate_determine_deflection('y');
-
- if (canStabilizeInverted() && current_orientation == F_INVERTED)
- {
- yawNavDeflection = -yawNavDeflection;
- }
- }
- else
- {
- yawNavDeflection = 0;
- }
-
- if (YAW_STABILIZATION_RUDDER && state_flags._.pitch_feedback)
- {
- gyroYawFeedback.WW = __builtin_mulus(yawkdrud, omegaAccum[2]);
+ gyroYawFeedback.WW = - __builtin_mulsu(rotationRateError[2], yawkdrud);
+ yawStabilization.WW = - __builtin_mulsu(tiltError[2], yawkprud); //
yaw orientation error in body frame
+ yawStabilization.WW += __builtin_mulsu(desiredRotationRateRadians[2],
yawkpfdfwd); // feed forward term
}
else
{
gyroYawFeedback.WW = 0;
+ yawStabilization.WW = 0;
}

rollStabilization.WW = 0; // default case is no roll rudder stabilization
if (settings._.RollStabilizationRudder && state_flags._.pitch_feedback)
{
- if (!desired_behavior._.inverted && !desired_behavior._.hover) // normal
- {
- rollStabilization.WW = __builtin_mulsu(rmat[6], rollkprud);
- }
- else if (desired_behavior._.inverted) // inverted
- {
- rollStabilization.WW = - __builtin_mulsu(rmat[6], rollkprud);
- }
- rollStabilization.WW -= __builtin_mulus(rollkdrud, omegaAccum[1]);
+ rollStabilization.WW = - __builtin_mulsu(tiltError[1], rollkprud); //
this works right side up or upside down
}

if (state_flags._.pitch_feedback)
@@ -129,9 +114,9 @@
ail_rud_mix = 0;
}

- yaw_control = (int32_t)yawNavDeflection
- - (int32_t)gyroYawFeedback._.W1
+ yaw_control = (int32_t)gyroYawFeedback._.W1
+ (int32_t)rollStabilization._.W1
+ + (int32_t)yawStabilization._.W1
+ ail_rud_mix;
// Servo reversing is handled in servoMix.c
}
=======================================
--- /trunk/Tools/MAVLink/mavlink/message_definitions/v1.0/common.xml Fri
Jul 18 08:58:00 2014 UTC
+++ /trunk/Tools/MAVLink/mavlink/message_definitions/v1.0/common.xml Sat
Aug 1 04:50:37 2015 UTC
@@ -55,6 +55,9 @@
<entry value="16" name="MAV_AUTOPILOT_AEROB">
<description>Aerob -- http://aerob.ru</description>
</entry>
+ <entry value="17" name="MAV_AUTOPILOT_ASLUAV">
+ <description>ASLUAV autopilot --
http://www.asl.ethz.ch</description>
+ </entry>
</enum>
<enum name="MAV_TYPE">
<entry value="0" name="MAV_TYPE_GENERIC">
@@ -114,6 +117,13 @@
<entry value="18" name="MAV_TYPE_ONBOARD_CONTROLLER">
<description>Onboard companion controller</description>
</entry>
+ <entry value="19" name="MAV_TYPE_VTOL_DUOROTOR">
+ <description>Two-rotor VTOL using control surfaces in
vertical operation in addition. Tailsitter.</description>
+ </entry>
+ <entry value="20" name="MAV_TYPE_VTOL_QUADROTOR">
+ <description>Quad-rotor VTOL using a V-shaped quad
config in vertical operation. Tailsitter.</description>
+ </entry>
+ <!-- Entries up to 25 reserved for other VTOL airframes -->
</enum>
<!-- WARNING: MAV_ACTION Enum is no longer supported - has been
removed. Please use MAV_CMD -->
<enum name="MAV_MODE_FLAG">
@@ -396,6 +406,9 @@
<entry value="2097152" name="MAV_SYS_STATUS_AHRS">
<description>0x200000 AHRS subsystem
health</description>
</entry>
+ <entry value="4194304" name="MAV_SYS_STATUS_TERRAIN">
+ <description>0x400000 Terrain subsystem
health</description>
+ </entry>
</enum>
<enum name="MAV_FRAME">
<entry value="0" name="MAV_FRAME_GLOBAL">
@@ -413,12 +426,6 @@
<entry value="4" name="MAV_FRAME_LOCAL_ENU">
<description>Local coordinate frame, Z-down (x: east,
y: north, z: up)</description>
</entry>
- <entry value="5" name="MAV_FRAME_GLOBAL_INT">
- <description>Global coordinate frame with some fields
as scaled integers, WGS84 coordinate system. First value / x: latitude,
second value / y: longitude, third value / z: positive altitude over mean
sea level (MSL). Lat / Lon are scaled * 1E7 to avoid floating point
accuracy limitations.</description>
- </entry>
- <entry value="6" name="MAV_FRAME_GLOBAL_RELATIVE_ALT_INT">
- <description>Global coordinate frame with some fields
as scaled integers, WGS84 coordinate system, relative altitude over ground
with respect to the home position. First value / x: latitude, second value
/ y: longitude, third value / z: positive altitude with 0 being at the
altitude of the home location. Lat / Lon are scaled * 1E7 to avoid floating
point accuracy limitations.</description>
- </entry>
<entry value="7" name="MAV_FRAME_LOCAL_OFFSET_NED">
<description>Offset to the current local frame.
Anything expressed in this frame should be added to the current local frame
position.</description>
</entry>
@@ -428,6 +435,9 @@
<entry value="9" name="MAV_FRAME_BODY_OFFSET_NED">
<description>Offset in body NED frame. This makes sense
if adding setpoints to the current flight path, to avoid an obstacle - e.g.
useful to command 2 m/s^2 acceleration to the east.</description>
</entry>
+ <entry value="10" name="MAV_FRAME_GLOBAL_TERRAIN_ALT">
+ <description>Global coordinate frame with above terrain
level altitude. WGS84 coordinate system, relative altitude over terrain
with respect to the waypoint coordinate. First value / x: latitude in
degrees, second value / y: longitude in degrees, third value / z: positive
altitude in meters with 0 being at ground level in terrain
model.</description>
+ </entry>
</enum>
<enum name="MAVLINK_DATA_STREAM_TYPE">
<entry name="MAVLINK_DATA_STREAM_IMG_JPEG">
@@ -590,26 +600,12 @@
<param index="6">Longitude/Y of goal</param>
<param index="7">Altitude/Z of goal</param>
</entry>
- <entry value="90" name="MAV_CMD_NAV_GUIDED_LIMITS">
- <description>set limits for external
control</description>
- <param index="1">timeout - maximum time (in seconds)
that external controller will be allowed to control vehicle. 0 means no
timeout</param>
- <param index="2">absolute altitude min (in meters,
WGS84) - if vehicle moves below this alt, the command will be aborted and
the mission will continue. 0 means no lower altitude limit</param>
- <param index="3">absolute altitude max (in meters)- if
vehicle moves above this alt, the command will be aborted and the mission
will continue. 0 means no upper altitude limit</param>
- <param index="4">horizontal move limit (in meters,
WGS84) - if vehicle moves more than this distance from it's location at the
moment the command was executed, the command will be aborted and the
mission will continue. 0 means no horizontal altitude limit</param>
- <param index="5">Empty</param>
- <param index="6">Empty</param>
- <param index="7">Empty</param>
- </entry>
- <entry value="91" name="MAV_CMD_NAV_GUIDED_MASTER">
- <description>set id of master controller</description>
- <param index="1">System ID</param>
- <param index="2">Component ID</param>
- <param index="3">Empty</param>
- <param index="4">Empty</param>
- <param index="5">Empty</param>
- <param index="6">Empty</param>
- <param index="7">Empty</param>
- </entry>
+
+ <!-- IDs 90 and 91 are reserved until the end of 2014,
+ as they were used in some conflicting proposals
+ between PX4 and ardupilot and need to be kept
+ unused to prevent errors -->
+
<entry value="92" name="MAV_CMD_NAV_GUIDED_ENABLE">
<description>hand control over to an external
controller</description>
<param index="1">On / Off (> 0.5f on)</param>
@@ -770,6 +766,26 @@
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
+ <entry value="185" name="MAV_CMD_DO_FLIGHTTERMINATION">
+ <description>Terminate flight immediately</description>
+ <param index="1">Flight termination activated if >
0.5</param>
+ <param index="2">Empty</param>
+ <param index="3">Empty</param>
+ <param index="4">Empty</param>
+ <param index="5">Empty</param>
+ <param index="6">Empty</param>
+ <param index="7">Empty</param>
+ </entry>
+ <entry value="189" name="MAV_CMD_DO_LAND_START">
+ <description>Mission command to perform a landing.
This is used as a marker in a mission to tell the autopilot where a
sequence of mission items that represents a landing starts. It may also be
sent via a COMMAND_LONG to trigger a landing, in which case the nearest
(geographically) landing sequence in the mission will be used. The
Latitude/Longitude is optional, and may be set to 0/0 if not needed. If
specified then it will be used to help find the closest landing
sequence.</description>
+ <param index="1">Empty</param>
+ <param index="2">Empty</param>
+ <param index="3">Empty</param>
+ <param index="4">Empty</param>
+ <param index="5">Latitude</param>
+ <param index="6">Longitude</param>
+ <param index="7">Empty</param>
+ </entry>
<entry value="190" name="MAV_CMD_DO_RALLY_LAND">
<description>Mission command to perform a landing from
a rally point.</description>
<param index="1">Break altitude (meters)</param>
@@ -903,15 +919,37 @@

<entry value="220" name="MAV_CMD_DO_MOUNT_CONTROL_QUAT">
<description>Mission command to control a camera or
antenna mount, using a quaternion as reference.</description>
- <param index="1">q1 - quaternion param #1</param>
- <param index="2">q2 - quaternion param #2</param>
- <param index="3">q3 - quaternion param #3</param>
- <param index="4">q4 - quaternion param #4</param>
+ <param index="1">q1 - quaternion param #1, w (1 in
null-rotation)</param>
+ <param index="2">q2 - quaternion param #2, x (0 in
null-rotation)</param>
+ <param index="3">q3 - quaternion param #3, y (0 in
null-rotation)</param>
+ <param index="4">q4 - quaternion param #4, z (0 in
null-rotation)</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>

+ <entry value="221" name="MAV_CMD_DO_GUIDED_MASTER">
+ <description>set id of master controller</description>
+ <param index="1">System ID</param>
+ <param index="2">Component ID</param>
+ <param index="3">Empty</param>
+ <param index="4">Empty</param>
+ <param index="5">Empty</param>
+ <param index="6">Empty</param>
+ <param index="7">Empty</param>
+ </entry>
+
+ <entry value="222" name="MAV_CMD_DO_GUIDED_LIMITS">
+ <description>set limits for external control</description>
+ <param index="1">timeout - maximum time (in seconds) that
external controller will be allowed to control vehicle. 0 means no
timeout</param>
+ <param index="2">absolute altitude min (in meters, WGS84)
- if vehicle moves below this alt, the command will be aborted and the
mission will continue. 0 means no lower altitude limit</param>
+ <param index="3">absolute altitude max (in meters)- if
vehicle moves above this alt, the command will be aborted and the mission
will continue. 0 means no upper altitude limit</param>
+ <param index="4">horizontal move limit (in meters, WGS84)
- if vehicle moves more than this distance from it's location at the moment
the command was executed, the command will be aborted and the mission will
continue. 0 means no horizontal altitude limit</param>
+ <param index="5">Empty</param>
+ <param index="6">Empty</param>
+ <param index="7">Empty</param>
+ </entry>
+
<entry value="240" name="MAV_CMD_DO_LAST">
<description>NOP - This command is only used to mark
the upper limit of the DO commands in the enumeration</description>
<param index="1">Empty</param>
@@ -934,7 +972,7 @@
</entry>
<entry value="242"
name="MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS">
<description>Set sensor offsets. This command will be
only accepted if in pre-flight mode.</description>
- <param index="1">Sensor to adjust the offsets for: 0:
gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical
flow</param>
+ <param index="1">Sensor to adjust the offsets for: 0:
gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5:
second magnetometer</param>
<param index="2">X axis offset (or generic dimension
1), in the sensor's raw units</param>
<param index="3">Y axis offset (or generic dimension
2), in the sensor's raw units</param>
<param index="4">Z axis offset (or generic dimension
3), in the sensor's raw units</param>
@@ -986,6 +1024,64 @@
<param index="1">0:Spektrum</param>
<param index="2">0:Spektrum DSM2, 1:Spektrum
DSMX</param>
</entry>
+ <entry value="2000" name="MAV_CMD_IMAGE_START_CAPTURE">
+ <description>Start image capture sequence</description>
+ <param index="1">Duration between two consecutive
pictures (in seconds)</param>
+ <param index="2">Number of images to capture total - 0
for unlimited capture</param>
+ <param index="3">Resolution in megapixels (0.3 for
640x480, 1.3 for 1280x720, etc)</param>
+ </entry>
+
+ <entry value="2001" name="MAV_CMD_IMAGE_STOP_CAPTURE">
+ <description>Stop image capture sequence</description>
+ <param index="1">Reserved</param>
+ <param index="2">Reserved</param>
+ </entry>
+
+ <entry value="2500" name="MAV_CMD_VIDEO_START_CAPTURE">
+ <description>Starts video capture</description>
+ <param index="1">Camera ID (0 for all cameras), 1 for
first, 2 for second, etc.</param>
+ <param index="2">Frames per second</param>
+ <param index="3">Resolution in megapixels (0.3 for
640x480, 1.3 for 1280x720, etc)</param>
+ </entry>
+
+ <entry value="2501" name="MAV_CMD_VIDEO_STOP_CAPTURE">
+ <description>Stop the current video capture</description>
+ <param index="1">Reserved</param>
+ <param index="2">Reserved</param>
+ </entry>
+
+ <entry value="2800" name="MAV_CMD_PANORAMA_CREATE">
+ <description>Create a panorama at the current
position</description>
+ <param index="1">Viewing angle horizontal of the
panorama (in degrees, +- 0.5 the total angle)</param>
+ <param index="2">Viewing angle vertical of panorama (in
degrees)</param>
+ <param index="3">Speed of the horizontal rotation (in
degrees per second)</param>
+ <param index="4">Speed of the vertical rotation (in
degrees per second)</param>
+ </entry>
+
+ <!-- VALUES FROM 0-40000 are reserved for the common message
set. Values from 40000 to UINT16_MAX are available for dialects -->
+
+ <!-- BEGIN of payload range (30000 to 30999) -->
+ <entry value="30001" name="MAV_CMD_PAYLOAD_PREPARE_DEPLOY">
+ <description>Deploy payload on a Lat / Lon / Alt
position. This includes the navigation to reach the required release
position and velocity.</description>
+ <param index="1">Operation mode. 0: prepare single
payload deploy (overwriting previous requests), but do not execute it. 1:
execute payload deploy immediately (rejecting further deploy commands
during execution, but allowing abort). 2: add payload deploy to existing
deployment list.</param>
+ <param index="2">Desired approach vector in degrees
compass heading (0..360). A negative value indicates the system can define
the approach vector at will.</param>
+ <param index="3">Desired ground speed at release time.
This can be overriden by the airframe in case it needs to meet minimum
airspeed. A negative value indicates the system can define the ground speed
at will.</param>
+ <param index="4">Minimum altitude clearance to the
release position in meters. A negative value indicates the system can
define the clearance at will.</param>
+ <param index="5">Latitude unscaled for MISSION_ITEM or
in 1e7 degrees for MISSION_ITEM_INT</param>
+ <param index="6">Longitude unscaled for MISSION_ITEM or
in 1e7 degrees for MISSION_ITEM_INT</param>
+ <param index="7">Altitude, in meters WGS84</param>
+ </entry>
+ <entry value="30002" name="MAV_CMD_PAYLOAD_CONTROL_DEPLOY">
+ <description>Control the payload
deployment.</description>
+ <param index="1">Operation mode. 0: Abort deployment,
continue normal mission. 1: switch to payload deploment mode. 100: delete
first payload deployment request. 101: delete all payload deployment
requests.</param>
+ <param index="2">Reserved</param>
+ <param index="3">Reserved</param>
+ <param index="4">Reserved</param>
+ <param index="5">Reserved</param>
+ <param index="6">Reserved</param>
+ <param index="7">Reserved</param>
+ </entry>
+ <!-- END of payload range (30000 to 30999) -->
</enum>
<enum name="MAV_DATA_STREAM">
<description>Data stream IDs. A data stream is not a fixed
set of messages, but rather a
@@ -1258,6 +1354,93 @@
<description>Ultrasound altimeter, e.g. MaxBotix
units</description>
</entry>
</enum>
+ <enum name="MAV_PROTOCOL_CAPABILITY">
+ <description>Bitmask of (optional) autopilot capabilities
(64 bit). If a bit is set, the autopilot supports this
capability.</description>
+ <entry value="1"
name="MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT">
+ <description>Autopilot supports MISSION float message
type.</description>
+ </entry>
+ <entry value="2" name="MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT">
+ <description>Autopilot supports the new param float
message type.</description>
+ </entry>
+ <entry value="4" name="MAV_PROTOCOL_CAPABILITY_MISSION_INT">
+ <description>Autopilot supports MISSION_INT scaled
integer message type.</description>
+ </entry>
+ <entry value="8" name="MAV_PROTOCOL_CAPABILITY_COMMAND_INT">
+ <description>Autopilot supports COMMAND_INT scaled
integer message type.</description>
+ </entry>
+ <entry value="16" name="MAV_PROTOCOL_CAPABILITY_PARAM_UNION">
+ <description>Autopilot supports the new param union
message type.</description>
+ </entry>
+ <entry value="32" name="MAV_PROTOCOL_CAPABILITY_FTP">
+ <description>Autopilot supports the new param union
message type.</description>
+ </entry>
+ <entry value="64"
name="MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET">
+ <description>Autopilot supports commanding attitude
offboard.</description>
+ </entry>
+ <entry value="128"
name="MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED">
+ <description>Autopilot supports commanding position and
velocity targets in local NED frame.</description>
+ </entry>
+ <entry value="256"
name="MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT">
+ <description>Autopilot supports commanding position and
velocity targets in global scaled integers.</description>
+ </entry>
+ <entry value="512" name="MAV_PROTOCOL_CAPABILITY_TERRAIN">
+ <description>Autopilot supports terrain protocol / data
handling.</description>
+ </entry>
+ </enum>
+ <enum name="MAV_ESTIMATOR_TYPE">
+ <description>Enumeration of estimator types</description>
+ <entry value="1" name="MAV_ESTIMATOR_TYPE_NAIVE">
+ <description>This is a naive estimator without any real
covariance feedback.</description>
+ </entry>
+ <entry value="2" name="MAV_ESTIMATOR_TYPE_VISION">
+ <description>Computer vision based estimate. Might be up
to scale.</description>
+ </entry>
+ <entry value="3" name="MAV_ESTIMATOR_TYPE_VIO">
+ <description>Visual-inertial estimate.</description>
+ </entry>
+ <entry value="4" name="MAV_ESTIMATOR_TYPE_GPS">
+ <description>Plain GPS estimate.</description>
+ </entry>
+ <entry value="5" name="MAV_ESTIMATOR_TYPE_GPS_INS">
+ <description>Estimator integrating GPS and inertial
sensing.</description>
+ </entry>
+ </enum>
+ <enum name="MAV_BATTERY_TYPE">
+ <description>Enumeration of battery types</description>
+ <entry value="0" name="MAV_BATTERY_TYPE_UNKNOWN">
+ <description>Not specified.</description>
+ </entry>
+ <entry value="1" name="MAV_BATTERY_TYPE_LIPO">
+ <description>Lithium polymere battery</description>
+ </entry>
+ <entry value="2" name="MAV_BATTERY_TYPE_LIFE">
+ <description>Lithium ferrite battery</description>
+ </entry>
+ <entry value="3" name="MAV_BATTERY_TYPE_LION">
+ <description>Lithium-ION battery</description>
+ </entry>
+ <entry value="4" name="MAV_BATTERY_TYPE_NIMH">
+ <description>Nickel metal hydride battery</description>
+ </entry>
+ </enum>
+ <enum name="MAV_BATTERY_FUNCTION">
+ <description>Enumeration of battery functions</description>
+ <entry value="0" name="MAV_BATTERY_FUNCTION_UNKNOWN">
+ <description>Lithium polymere battery</description>
+ </entry>
+ <entry value="1" name="MAV_BATTERY_FUNCTION_ALL">
+ <description>Battery supports all flight
systems</description>
+ </entry>
+ <entry value="2" name="MAV_BATTERY_FUNCTION_PROPULSION">
+ <description>Battery for the propulsion
system</description>
+ </entry>
+ <entry value="3" name="MAV_BATTERY_FUNCTION_AVIONICS">
+ <description>Avionics battery</description>
+ </entry>
+ <entry value="4" name="MAV_BATTERY_TYPE_PAYLOAD">
+ <description>Payload battery</description>
+ </entry>
+ </enum>
</enums>
<messages>
<message id="0" name="HEARTBEAT">
@@ -1293,7 +1476,7 @@
<!-- FIXME to be removed / merged with SYSTEM_TIME -->
<message id="4" name="PING">
<description>A ping message either requesting or responding
to a ping. This allows to measure the system latencies, including serial
port, radio modem and UDP connections.</description>
- <field type="uint64_t" name="time_usec">Unix timestamp in
microseconds</field>
+ <field type="uint64_t" name="time_usec">Unix timestamp in
microseconds or since system boot if smaller than MAVLink epoch
(1.1.2009)</field>
<field type="uint32_t" name="seq">PING sequence</field>
<field type="uint8_t" name="target_system">0: request ping
from all receiving systems, if greater than 0: message is a ping response
and number is the system id of the requesting system</field>
<field type="uint8_t" name="target_component">0: request
ping from all receiving components, if greater than 0: message is a ping
response and number is the system id of the requesting system</field>
@@ -1321,6 +1504,7 @@
<field type="uint8_t" name="base_mode" enum="MAV_MODE">The
new base mode</field>
<field type="uint32_t" name="custom_mode">The new
autopilot-specific mode. This field can be ignored by an autopilot.</field>
</message>
+ <!-- reserved for PARAM_VALUE_UNION -->
<message id="20" name="PARAM_REQUEST_READ">
<description>Request to read the onboard parameter with the
param_id string id. Onboard parameters are stored as key[const char*] ->
value[float]. This allows to send a parameter to any other component (such
as the GCS) without the need of previous knowledge of possible parameter
names. Thus the same GCS can store different parameters for different
autopilots. See also http://qgroundcontrol.org/parameter_interface for a
full documentation of QGroundControl and IMU code.</description>
<field type="uint8_t" name="target_system">System ID</field>
@@ -1424,12 +1608,12 @@
<field type="float" name="yawspeed">Yaw angular speed
(rad/s)</field>
</message>
<message id="31" name="ATTITUDE_QUATERNION">
- <description>The attitude in the aeronautical frame
(right-handed, Z-down, X-front, Y-right), expressed as
quaternion.</description>
+ <description>The attitude in the aeronautical frame
(right-handed, Z-down, X-front, Y-right), expressed as quaternion.
Quaternion order is w, x, y, z and a zero rotation would be expressed as (1
0 0 0).</description>
<field type="uint32_t" name="time_boot_ms">Timestamp
(milliseconds since system boot)</field>
- <field type="float" name="q1">Quaternion component 1</field>
- <field type="float" name="q2">Quaternion component 2</field>
- <field type="float" name="q3">Quaternion component 3</field>
- <field type="float" name="q4">Quaternion component 4</field>
+ <field type="float" name="q1">Quaternion component 1, w (1
in null-rotation)</field>
+ <field type="float" name="q2">Quaternion component 2, x (0
in null-rotation)</field>
+ <field type="float" name="q3">Quaternion component 3, y (0
in null-rotation)</field>
+ <field type="float" name="q4">Quaternion component 4, z (0
in null-rotation)</field>
<field type="float" name="rollspeed">Roll angular speed
(rad/s)</field>
<field type="float" name="pitchspeed">Pitch angular speed
(rad/s)</field>
<field type="float" name="yawspeed">Yaw angular speed
(rad/s)</field>
@@ -1450,7 +1634,7 @@
<field type="uint32_t" name="time_boot_ms">Timestamp
(milliseconds since system boot)</field>
<field type="int32_t" name="lat">Latitude, expressed as *
1E7</field>
<field type="int32_t" name="lon">Longitude, expressed as *
1E7</field>
- <field type="int32_t" name="alt">Altitude in meters,
expressed as * 1000 (millimeters), above MSL</field>
+ <field type="int32_t" name="alt">Altitude in meters,
expressed as * 1000 (millimeters), WGS84 (not AMSL)</field>
<field type="int32_t" name="relative_alt">Altitude above
ground in meters, expressed as * 1000 (millimeters)</field>
<field type="int16_t" name="vx">Ground X Speed (Latitude),
expressed as m/s * 100</field>
<field type="int16_t" name="vy">Ground Y Speed (Longitude),
expressed as m/s * 100</field>
@@ -1585,40 +1769,6 @@
<field type="int32_t" name="longitude">Longitude (WGS84),
in degrees * 1E7</field>
<field type="int32_t" name="altitude">Altitude (WGS84), in
meters * 1000 (positive for up)</field>
</message>
- <message id="50" name="SET_LOCAL_POSITION_SETPOINT">
- <description>Set the setpoint for a local position
controller. This is the position in local coordinates the MAV should fly
to. This message is sent by the path/MISSION planner to the onboard
position controller. As some MAVs have a degree of freedom in yaw (e.g. all
helicopters/quadrotors), the desired yaw angle is part of the
message.</description>
- <field type="uint8_t" name="target_system">System ID</field>
- <field type="uint8_t" name="target_component">Component
ID</field>
- <field type="uint8_t" name="coordinate_frame">Coordinate
frame - valid values are only MAV_FRAME_LOCAL_NED or
MAV_FRAME_LOCAL_ENU</field>
- <field type="float" name="x">x position</field>
- <field type="float" name="y">y position</field>
- <field type="float" name="z">z position</field>
- <field type="float" name="yaw">Desired yaw angle</field>
- </message>
- <message id="51" name="LOCAL_POSITION_SETPOINT">
- <description>Transmit the current local setpoint of the
controller to other MAVs (collision avoidance) and to the GCS.</description>
- <field type="uint8_t" name="coordinate_frame">Coordinate
frame - valid values are only MAV_FRAME_LOCAL_NED or
MAV_FRAME_LOCAL_ENU</field>
- <field type="float" name="x">x position</field>
- <field type="float" name="y">y position</field>
- <field type="float" name="z">z position</field>
- <field type="float" name="yaw">Desired yaw angle</field>
- </message>
- <message id="52" name="GLOBAL_POSITION_SETPOINT_INT">
- <description>Transmit the current local setpoint of the
controller to other MAVs (collision avoidance) and to the GCS.</description>
- <field type="uint8_t" name="coordinate_frame">Coordinate
frame - valid values are only MAV_FRAME_GLOBAL or
MAV_FRAME_GLOBAL_RELATIVE_ALT</field>
- <field type="int32_t" name="latitude">Latitude (WGS84), in
degrees * 1E7</field>
- <field type="int32_t" name="longitude">Longitude (WGS84),
in degrees * 1E7</field>
- <field type="int32_t" name="altitude">Altitude (WGS84), in
meters * 1000 (positive for up)</field>
- <field type="int16_t" name="yaw">Desired yaw angle in
degrees * 100</field>
- </message>
- <message id="53" name="SET_GLOBAL_POSITION_SETPOINT_INT">
- <description>Set the current global position
setpoint.</description>
- <field type="uint8_t" name="coordinate_frame">Coordinate
frame - valid values are only MAV_FRAME_GLOBAL or
MAV_FRAME_GLOBAL_RELATIVE_ALT</field>
- <field type="int32_t" name="latitude">Latitude (WGS84), in
degrees * 1E7</field>
- <field type="int32_t" name="longitude">Longitude (WGS84),
in degrees * 1E7</field>
- <field type="int32_t" name="altitude">Altitude (WGS84), in
meters * 1000 (positive for up)</field>
- <field type="int16_t" name="yaw">Desired yaw angle in
degrees * 100</field>
- </message>
<message id="54" name="SAFETY_SET_ALLOWED_AREA">
<description>Set a safety zone (volume), which is defined
by two corners of a cube. This message can be used to tell the MAV which
setpoints/MISSIONs to accept and which to reject. Safety areas are often
enforced by national or competition regulations.</description>
<field type="uint8_t" name="target_system">System ID</field>
@@ -1641,56 +1791,14 @@
<field type="float" name="p2y">y position 2 / Longitude
2</field>
<field type="float" name="p2z">z position 2 / Altitude
2</field>
</message>
- <message id="56" name="SET_ROLL_PITCH_YAW_THRUST">
- <description>Set roll, pitch and yaw.</description>
- <field type="uint8_t" name="target_system">System ID</field>
- <field type="uint8_t" name="target_component">Component
ID</field>
- <field type="float" name="roll">Desired roll angle in
radians</field>
- <field type="float" name="pitch">Desired pitch angle in
radians</field>
- <field type="float" name="yaw">Desired yaw angle in
radians</field>
- <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1</field>
- </message>
- <message id="57" name="SET_ROLL_PITCH_YAW_SPEED_THRUST">
- <description>Set roll, pitch and yaw.</description>
- <field type="uint8_t" name="target_system">System ID</field>
- <field type="uint8_t" name="target_component">Component
ID</field>
- <field type="float" name="roll_speed">Desired roll angular
speed in rad/s</field>
- <field type="float" name="pitch_speed">Desired pitch
angular speed in rad/s</field>
- <field type="float" name="yaw_speed">Desired yaw angular
speed in rad/s</field>
- <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1</field>
- </message>
- <message id="58" name="ROLL_PITCH_YAW_THRUST_SETPOINT">
- <description>Setpoint in roll, pitch, yaw currently active
on the system.</description>
- <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
- <field type="float" name="roll">Desired roll angle in
radians</field>
- <field type="float" name="pitch">Desired pitch angle in
radians</field>
- <field type="float" name="yaw">Desired yaw angle in
radians</field>
- <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1</field>
- </message>
- <message id="59" name="ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT">
- <description>Setpoint in rollspeed, pitchspeed, yawspeed
currently active on the system.</description>
- <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
- <field type="float" name="roll_speed">Desired roll angular
speed in rad/s</field>
- <field type="float" name="pitch_speed">Desired pitch
angular speed in rad/s</field>
- <field type="float" name="yaw_speed">Desired yaw angular
speed in rad/s</field>
- <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1</field>
- </message>
- <message id="60" name="SET_QUAD_MOTORS_SETPOINT">
- <description>Setpoint in the four motor speeds</description>
- <field type="uint8_t" name="target_system">System ID of the
system that should set these motor commands</field>
- <field type="uint16_t" name="motor_front_nw">Front motor in
+ configuration, front left motor in x configuration</field>
- <field type="uint16_t" name="motor_right_ne">Right motor in
+ configuration, front right motor in x configuration</field>
- <field type="uint16_t" name="motor_back_se">Back motor in +
configuration, back right motor in x configuration</field>
- <field type="uint16_t" name="motor_left_sw">Left motor in +
configuration, back left motor in x configuration</field>
- </message>
- <message id="61" name="SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST">
- <description>Setpoint for up to four quadrotors in a group
/ wing</description>
- <field type="uint8_t" name="group">ID of the quadrotor
group (0 - 255, up to 256 groups supported)</field>
- <field type="uint8_t" name="mode">ID of the flight mode (0
- 255, up to 256 modes supported)</field>
- <field type="int16_t[4]" name="roll">Desired roll angle in
radians +-PI (+-INT16_MAX)</field>
- <field type="int16_t[4]" name="pitch">Desired pitch angle
in radians +-PI (+-INT16_MAX)</field>
- <field type="int16_t[4]" name="yaw">Desired yaw angle in
radians, scaled to int16 +-PI (+-INT16_MAX)</field>
- <field type="uint16_t[4]" name="thrust">Collective thrust,
scaled to uint16 (0..UINT16_MAX)</field>
+ <message id="61" name="ATTITUDE_QUATERNION_COV">
+ <description>The attitude in the aeronautical frame
(right-handed, Z-down, X-front, Y-right), expressed as quaternion.
Quaternion order is w, x, y, z and a zero rotation would be expressed as (1
0 0 0).</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp
(milliseconds since system boot)</field>
+ <field type="float[4]" name="q">Quaternion components, w,
x, y, z (1 0 0 0 is the null-rotation)</field>
+ <field type="float" name="rollspeed">Roll angular speed
(rad/s)</field>
+ <field type="float" name="pitchspeed">Pitch angular speed
(rad/s)</field>
+ <field type="float" name="yawspeed">Yaw angular speed
(rad/s)</field>
+ <field type="float[9]" name="covariance">Attitude
covariance</field>
</message>
<message id="62" name="NAV_CONTROLLER_OUTPUT">
<description>Outputs of the APM navigation controller. The
primary use of this message is to check the response and signs of the
controller before actual flight and to assist with tuning controller
parameters.</description>
@@ -1703,29 +1811,32 @@
<field type="float" name="aspd_error">Current airspeed
error in meters/second</field>
<field type="float" name="xtrack_error">Current crosstrack
error on x-y plane in meters</field>
</message>
- <message id="63" name="SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST">
- <description>Setpoint for up to four quadrotors in a group
/ wing</description>
- <field type="uint8_t" name="group">ID of the quadrotor
group (0 - 255, up to 256 groups supported)</field>
- <field type="uint8_t" name="mode">ID of the flight mode (0
- 255, up to 256 modes supported)</field>
- <field type="uint8_t[4]" name="led_red">RGB red channel
(0-255)</field>
- <field type="uint8_t[4]" name="led_blue">RGB green channel
(0-255)</field>
- <field type="uint8_t[4]" name="led_green">RGB blue channel
(0-255)</field>
- <field type="int16_t[4]" name="roll">Desired roll angle in
radians +-PI (+-INT16_MAX)</field>
- <field type="int16_t[4]" name="pitch">Desired pitch angle
in radians +-PI (+-INT16_MAX)</field>
- <field type="int16_t[4]" name="yaw">Desired yaw angle in
radians, scaled to int16 +-PI (+-INT16_MAX)</field>
- <field type="uint16_t[4]" name="thrust">Collective thrust,
scaled to uint16 (0..UINT16_MAX)</field>
+ <message id="63" name="GLOBAL_POSITION_INT_COV">
+ <description>The filtered global position (e.g. fused GPS
and accelerometers). The position is in GPS-frame (right-handed, Z-up). It
is designed as scaled integer message since the resolution of float is not
sufficient. NOTE: This message is intended for onboard networks / companion
computers and higher-bandwidth links and optimized for accuracy and
completeness. Please use the GLOBAL_POSITION_INT message for a minimal
subset.</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp
(milliseconds since system boot)</field>
+ <field type="uint64_t" name="time_utc">Timestamp
(microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by
the precision time source of a GPS receiver.</field>
+ <field type="uint8_t" name="estimator_type"
enum="MAV_ESTIMATOR_TYPE">Class id of the estimator this estimate
originated from.</field>
+ <field type="int32_t" name="lat">Latitude, expressed as
degrees * 1E7</field>
+ <field type="int32_t" name="lon">Longitude, expressed as
degrees * 1E7</field>
+ <field type="int32_t" name="alt">Altitude in meters,
expressed as * 1000 (millimeters), above MSL</field>
+ <field type="int32_t" name="relative_alt">Altitude above
ground in meters, expressed as * 1000 (millimeters)</field>
+ <field type="float" name="vx">Ground X Speed (Latitude),
expressed as m/s</field>
+ <field type="float" name="vy">Ground Y Speed (Longitude),
expressed as m/s</field>
+ <field type="float" name="vz">Ground Z Speed (Altitude),
expressed as m/s</field>
+ <field type="float[36]" name="covariance">Covariance matrix
(first six entries are the first ROW, next six entries are the second row,
etc.)</field>
</message>
- <message id="64" name="STATE_CORRECTION">
- <description>Corrects the systems state by adding an error
correction term to the position and velocity, and by rotating the attitude
by a correction angle.</description>
- <field type="float" name="xErr">x position error</field>
- <field type="float" name="yErr">y position error</field>
- <field type="float" name="zErr">z position error</field>
- <field type="float" name="rollErr">roll error
(radians)</field>
- <field type="float" name="pitchErr">pitch error
(radians)</field>
- <field type="float" name="yawErr">yaw error
(radians)</field>
- <field type="float" name="vxErr">x velocity</field>
- <field type="float" name="vyErr">y velocity</field>
- <field type="float" name="vzErr">z velocity</field>
+ <message id="64" name="LOCAL_POSITION_NED_COV">
+ <description>The filtered local position (e.g. fused
computer vision and accelerometers). Coordinate frame is right-handed,
Z-axis down (aeronautical frame, NED / north-east-down
convention)</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp
(milliseconds since system boot)</field>
+ <field type="uint64_t" name="time_utc">Timestamp
(microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by
the precision time source of a GPS receiver.</field>
+ <field type="uint8_t" name="estimator_type"
enum="MAV_ESTIMATOR_TYPE">Class id of the estimator this estimate
originated from.</field>
+ <field type="float" name="x">X Position</field>
+ <field type="float" name="y">Y Position</field>
+ <field type="float" name="z">Z Position</field>
+ <field type="float" name="vx">X Speed</field>
+ <field type="float" name="vy">Y Speed</field>
+ <field type="float" name="vz">Z Speed</field>
+ <field type="float[36]" name="covariance">Covariance matrix
(first six entries are the first ROW, next six entries are the second row,
etc.)</field>
</message>
<message id="65" name="RC_CHANNELS">
<description>The PPM values of the RC channels received.
The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000
microseconds: 100%. Individual receivers/transmitters might violate this
specification.</description>
@@ -1785,6 +1896,24 @@
<field type="uint16_t" name="chan7_raw">RC channel 7 value,
in microseconds. A value of UINT16_MAX means to ignore this field.</field>
<field type="uint16_t" name="chan8_raw">RC channel 8 value,
in microseconds. A value of UINT16_MAX means to ignore this field.</field>
</message>
+ <message id="73" name="MISSION_ITEM_INT">
+ <description>Message encoding a mission item. This message
is emitted to announce
+ the presence of a mission item and to set a mission item
on the system. The mission item can be either in x, y, z meters (type:
LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed
(NED), global frame is Z-up, right handed (ENU). See
alsohttp://qgroundcontrol.org/mavlink/waypoint_protocol.</description>
+ <field type="uint8_t" name="target_system">System ID</field>
+ <field type="uint8_t" name="target_component">Component
ID</field>
+ <field type="uint16_t" name="seq">Waypoint ID (sequence
number). Starts at zero. Increases monotonically for each waypoint, no gaps
in the sequence (0,1,2,3,4).</field>
+ <field type="uint8_t" name="frame">The coordinate system of
the MISSION. see MAV_FRAME in mavlink_types.h</field>
+ <field type="uint16_t" name="command">The scheduled action
for the MISSION. see MAV_CMD in common.xml MAVLink specs</field>
+ <field type="uint8_t" name="current">false:0, true:1</field>
+ <field type="uint8_t" name="autocontinue">autocontinue to
next wp</field>
+ <field type="float" name="param1">PARAM1, see MAV_CMD
enum</field>
+ <field type="float" name="param2">PARAM2, see MAV_CMD
enum</field>
+ <field type="float" name="param3">PARAM3, see MAV_CMD
enum</field>
+ <field type="float" name="param4">PARAM4, see MAV_CMD
enum</field>
+ <field type="int32_t" name="x">PARAM5 / local: x position in
meters * 1e4, global: latitude in degrees * 10^7</field>
+ <field type="int32_t" name="y">PARAM6 / y position: local: x
position in meters * 1e4, global: longitude in degrees *10^7</field>
+ <field type="float" name="z">PARAM7 / z position: global:
altitude in meters (relative or absolute, depending on frame.</field>
+ </message>
<message id="74" name="VFR_HUD">
<description>Metrics typically displayed on a HUD for fixed
wing aircraft</description>
<field type="float" name="airspeed">Current airspeed in
m/s</field>
@@ -1794,6 +1923,22 @@
<field type="float" name="alt">Current altitude (MSL), in
meters</field>
<field type="float" name="climb">Current climb rate in
meters/second</field>
</message>
+ <message id="75" name="COMMAND_INT">
+ <description>Message encoding a command with parameters as
scaled integers. Scaling depends on the actual command value.</description>
+ <field type="uint8_t" name="target_system">System ID</field>
+ <field type="uint8_t" name="target_component">Component
ID</field>
+ <field type="uint8_t" name="frame">The coordinate system of
the COMMAND. see MAV_FRAME in mavlink_types.h</field>
+ <field type="uint16_t" name="command">The scheduled action
for the mission item. see MAV_CMD in common.xml MAVLink specs</field>
+ <field type="uint8_t" name="current">false:0, true:1</field>
+ <field type="uint8_t" name="autocontinue">autocontinue to
next wp</field>
+ <field type="float" name="param1">PARAM1, see MAV_CMD
enum</field>
+ <field type="float" name="param2">PARAM2, see MAV_CMD
enum</field>
+ <field type="float" name="param3">PARAM3, see MAV_CMD
enum</field>
+ <field type="float" name="param4">PARAM4, see MAV_CMD
enum</field>
+ <field type="int32_t" name="x">PARAM5 / local: x position in
meters * 1e4, global: latitude in degrees * 10^7</field>
+ <field type="int32_t" name="y">PARAM6 / local: y position in
meters * 1e4, global: longitude in degrees * 10^7</field>
+ <field type="float" name="z">PARAM7 / z position: global:
altitude in meters (relative or absolute, depending on frame.</field>
+ </message>
<message id="76" name="COMMAND_LONG">
<description>Send a command with up to seven parameters to
the MAV</description>
<field type="uint8_t" name="target_system">System which
should execute the command</field>
@@ -1813,14 +1958,6 @@
<field type="uint16_t" name="command"
enum="MAV_CMD">Command ID, as defined by MAV_CMD enum.</field>
<field type="uint8_t" name="result">See MAV_RESULT
enum</field>
</message>
- <message id="80" name="ROLL_PITCH_YAW_RATES_THRUST_SETPOINT">
- <description>Setpoint in roll, pitch, yaw rates and thrust
currently active on the system.</description>
- <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
- <field type="float" name="roll_rate">Desired roll rate in
radians per second</field>
- <field type="float" name="pitch_rate">Desired pitch rate in
radians per second</field>
- <field type="float" name="yaw_rate">Desired yaw rate in
radians per second</field>
- <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1</field>
- </message>
<message id="81" name="MANUAL_SETPOINT">
<description>Setpoint in roll, pitch, yaw and thrust from the
operator</description>
<field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
@@ -1831,11 +1968,21 @@
<field type="uint8_t" name="mode_switch">Flight mode switch
position, 0.. 255</field>
<field type="uint8_t" name="manual_override_switch">Override
mode switch position, 0.. 255</field>
</message>
- <message id="82" name="ATTITUDE_SETPOINT_EXTERNAL">
+ <message id="82" name="SET_ATTITUDE_TARGET">
<description>Set the vehicle attitude and body angular
rates.</description>
<field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component
ID</field>
+ <field type="uint8_t" name="type_mask">Mappings: If any of
these bits are set, the corresponding input should be ignored: bit 1: body
roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6:
reserved, bit 7: throttle, bit 8: attitude</field>
+ <field type="float[4]" name="q">Attitude quaternion (w, x,
y, z order, zero-rotation is 1, 0, 0, 0)</field>
+ <field type="float" name="body_roll_rate">Body roll rate in
radians per second</field>
+ <field type="float" name="body_pitch_rate">Body roll rate
in radians per second</field>
+ <field type="float" name="body_yaw_rate">Body roll rate in
radians per second</field>
+ <field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)</field>
+ </message>
+ <message id="83" name="ATTITUDE_TARGET">
+ <description>Set the vehicle attitude and body angular
rates.</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
<field type="uint8_t" name="type_mask">Mappings: If any of
these bits are set, the corresponding input should be ignored: bit 1: body
roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7:
reserved, bit 8: attitude</field>
<field type="float[4]" name="q">Attitude quaternion (w, x,
y, z order, zero-rotation is 1, 0, 0, 0)</field>
<field type="float" name="body_roll_rate">Body roll rate in
radians per second</field>
@@ -1843,13 +1990,13 @@
<field type="float" name="body_yaw_rate">Body roll rate in
radians per second</field>
<field type="float" name="thrust">Collective thrust,
normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)</field>
</message>
- <message id="83" name="LOCAL_NED_POSITION_SETPOINT_EXTERNAL">
+ <message id="84" name="SET_POSITION_TARGET_LOCAL_NED">
<description>Set vehicle position, velocity and
acceleration setpoint in local frame.</description>
<field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component
ID</field>
- <field type="uint8_t" name="coordinate_frame"
enum="MAV_FRAME">Valid options are: MAV_FRAME_LOCAL_NED,
MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6,
MAV_FRAME_BODY_OFFSET_NED = 7</field>
- <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint</field>
+ <field type="uint8_t" name="coordinate_frame"
enum="MAV_FRAME">Valid options are: MAV_FRAME_LOCAL_NED = 1,
MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8,
MAV_FRAME_BODY_OFFSET_NED = 9</field>
+ <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw
rate</field>
<field type="float" name="x">X Position in NED frame in
meters</field>
<field type="float" name="y">Y Position in NED frame in
meters</field>
<field type="float" name="z">Z Position in NED frame in
meters (note, altitude is negative in NED)</field>
@@ -1859,22 +2006,61 @@
<field type="float" name="afx">X acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
<field type="float" name="afy">Y acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
<field type="float" name="afz">Z acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="yaw">yaw setpoint in rad</field>
+ <field type="float" name="yaw_rate">yaw rate setpoint in
rad/s</field>
</message>
- <message id="84" name="GLOBAL_POSITION_SETPOINT_EXTERNAL_INT">
+ <message id="85" name="POSITION_TARGET_LOCAL_NED">
+ <description>Set vehicle position, velocity and
acceleration setpoint in local frame.</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot</field>
+ <field type="uint8_t" name="coordinate_frame"
enum="MAV_FRAME">Valid options are: MAV_FRAME_LOCAL_NED = 1,
MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8,
MAV_FRAME_BODY_OFFSET_NED = 9</field>
+ <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw
rate</field>
+ <field type="float" name="x">X Position in NED frame in
meters</field>
+ <field type="float" name="y">Y Position in NED frame in
meters</field>
+ <field type="float" name="z">Z Position in NED frame in
meters (note, altitude is negative in NED)</field>
+ <field type="float" name="vx">X velocity in NED frame in
meter / s</field>
+ <field type="float" name="vy">Y velocity in NED frame in
meter / s</field>
+ <field type="float" name="vz">Z velocity in NED frame in
meter / s</field>
+ <field type="float" name="afx">X acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="afy">Y acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="afz">Z acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="yaw">yaw setpoint in rad</field>
+ <field type="float" name="yaw_rate">yaw rate setpoint in
rad/s</field>
+ </message>
+ <message id="86" name="SET_POSITION_TARGET_GLOBAL_INT">
<description>Set vehicle position, velocity and
acceleration setpoint in the WGS84 coordinate system.</description>
<field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot. The rationale for the timestamp in the
setpoint is to allow the system to compensate for the transport delay of
the setpoint. This allows the system to compensate processing
latency.</field>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component
ID</field>
- <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint</field>
+ <field type="uint8_t" name="coordinate_frame"
enum="MAV_FRAME">Valid options are: MAV_FRAME_GLOBAL_INT = 5,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT =
11</field>
+ <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw
rate</field>
<field type="int32_t" name="lat_int">X Position in WGS84
frame in 1e7 * meters</field>
<field type="int32_t" name="lon_int">Y Position in WGS84
frame in 1e7 * meters</field>
- <field type="float" name="alt">Altitude in WGS84, not
AMSL</field>
+ <field type="float" name="alt">Altitude in meters in WGS84
altitude, not AMSL if absolute or relative, above terrain if
GLOBAL_TERRAIN_ALT_INT</field>
<field type="float" name="vx">X velocity in NED frame in
meter / s</field>
<field type="float" name="vy">Y velocity in NED frame in
meter / s</field>
<field type="float" name="vz">Z velocity in NED frame in
meter / s</field>
<field type="float" name="afx">X acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
<field type="float" name="afy">Y acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
<field type="float" name="afz">Z acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="yaw">yaw setpoint in rad</field>
+ <field type="float" name="yaw_rate">yaw rate setpoint in
rad/s</field>
+ </message>
+ <message id="87" name="POSITION_TARGET_GLOBAL_INT">
+ <description>Set vehicle position, velocity and
acceleration setpoint in the WGS84 coordinate system.</description>
+ <field type="uint32_t" name="time_boot_ms">Timestamp in
milliseconds since system boot. The rationale for the timestamp in the
setpoint is to allow the system to compensate for the transport delay of
the setpoint. This allows the system to compensate processing
latency.</field>
+ <field type="uint8_t" name="coordinate_frame"
enum="MAV_FRAME">Valid options are: MAV_FRAME_GLOBAL_INT = 5,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT =
11</field>
+ <field type="uint16_t" name="type_mask">Bitmask to indicate
which dimensions should be ignored by the vehicle: a value of
0b0000000000000000 or 0b0000001000000000 indicates that none of the
setpoint dimensions should be ignored. If bit 10 is set the floats afx afy
afz should be interpreted as force instead of acceleration. Mapping: bit 1:
x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8:
ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw
rate</field>
+ <field type="int32_t" name="lat_int">X Position in WGS84
frame in 1e7 * meters</field>
+ <field type="int32_t" name="lon_int">Y Position in WGS84
frame in 1e7 * meters</field>
+ <field type="float" name="alt">Altitude in meters in WGS84
altitude, not AMSL if absolute or relative, above terrain if
GLOBAL_TERRAIN_ALT_INT</field>
+ <field type="float" name="vx">X velocity in NED frame in
meter / s</field>
+ <field type="float" name="vy">Y velocity in NED frame in
meter / s</field>
+ <field type="float" name="vz">Z velocity in NED frame in
meter / s</field>
+ <field type="float" name="afx">X acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="afy">Y acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="afz">Z acceleration or force (if
bit 10 of type_mask is set) in NED frame in meter / s^2 or N</field>
+ <field type="float" name="yaw">yaw setpoint in rad</field>
+ <field type="float" name="yaw_rate">yaw rate setpoint in
rad/s</field>
</message>
<message id="89" name="LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET">
<description>The offset in X, Y, Z and yaw between the
LOCAL_POSITION_NED messages of MAV X and the global coordinate frame in NED
coordinates. Coordinate frame is right-handed, Z-axis down (aeronautical
frame, NED / north-east-down convention)</description>
@@ -1998,14 +2184,20 @@
<field type="float" name="temperature">Temperature in degrees
celsius</field>
<field type="uint16_t" name="fields_updated">Bitmask for
fields that have updated since last message, bit 0 = xacc, bit 12:
temperature</field>
</message>
- <message id="106" name="OMNIDIRECTIONAL_FLOW">
- <description>Optical flow from an omnidirectional flow sensor
(e.g. PX4FLOW with wide angle lens)</description>
+ <message id="106" name="OPTICAL_FLOW_RAD">
+ <description>Optical flow from an angular rate flow sensor
(e.g. PX4FLOW or mouse sensor)</description>
<field type="uint64_t" name="time_usec">Timestamp
(microseconds, synced to UNIX time or since system boot)</field>
<field type="uint8_t" name="sensor_id">Sensor ID</field>
- <field type="int16_t[10]" name="left">Flow in deci pixels (1
= 0.1 pixel) on left hemisphere</field>
- <field type="int16_t[10]" name="right">Flow in deci pixels (1
= 0.1 pixel) on right hemisphere</field>
- <field type="uint8_t" name="quality">Optical flow quality /
confidence. 0: bad, 255: maximum quality</field>
- <field type="float" name="front_distance_m">Front distance in
meters. Positive value (including zero): distance known. Negative value:
Unknown distance</field>
+ <field type="uint32_t" name="integration_time_us">Integration
time in microseconds. Divide integrated_x and integrated_y by the
integration time to obtain average flow. The integration time also
indicates the.</field>
+ <field type="float" name="integrated_x">Flow in radians
around X axis (Sensor RH rotation about the X axis induces a positive flow.
Sensor linear motion along the positive Y axis induces a negative
flow.)</field>
+ <field type="float" name="integrated_y">Flow in radians
around Y axis (Sensor RH rotation about the Y axis induces a positive flow.
Sensor linear motion along the positive X axis induces a positive
flow.)</field>
+ <field type="float" name="integrated_xgyro">RH rotation
around X axis (rad)</field>
+ <field type="float" name="integrated_ygyro">RH rotation
around Y axis (rad)</field>
+ <field type="float" name="integrated_zgyro">RH rotation
around Z axis (rad)</field>
+ <field type="int16_t" name="temperature">Temperature * 100 in
centi-degrees Celsius</field>
+ <field type="uint8_t" name="quality">Optical flow quality /
confidence. 0: no valid flow, 255: maximum quality</field>
+ <field type="uint32_t" name="time_delta_distance_us">Time in
microseconds since the distance was sampled.</field>
+ <field type="float" name="distance">Distance to the center of
the flow field in meters. Positive value (including zero): distance known.
Negative value: Unknown distance.</field>
</message>

<message id="107" name="HIL_SENSOR"> <description>The IMU
readings in SI units in NED body frame</description>
@@ -2028,10 +2220,10 @@

<message name="SIM_STATE" id="108">
<description>Status of simulation environment, if used</description>
- <field type="float" name="q1">True attitude quaternion
component 1</field>
- <field type="float" name="q2">True attitude quaternion
component 2</field>
- <field type="float" name="q3">True attitude quaternion
component 3</field>
- <field type="float" name="q4">True attitude quaternion
component 4</field>
+ <field type="float" name="q1">True attitude quaternion
component 1, w (1 in null-rotation)</field>
+ <field type="float" name="q2">True attitude quaternion
component 2, x (0 in null-rotation)</field>
+ <field type="float" name="q3">True attitude quaternion
component 3, y (0 in null-rotation)</field>
+ <field type="float" name="q4">True attitude quaternion
component 4, z (0 in null-rotation)</field>
<field type="float" name="roll">Attitude roll expressed as
Euler angles, not recommended except for human-readable outputs</field>
<field type="float" name="pitch">Attitude pitch expressed as
Euler angles, not recommended except for human-readable outputs</field>
<field type="float" name="yaw">Attitude yaw expressed as Euler
angles, not recommended except for human-readable outputs</field>
@@ -2062,25 +2254,18 @@
<field type="uint16_t" name="fixed">count of error corrected
packets</field>
</message>

- <message id="110" name="FILE_TRANSFER_START">
- <description>Begin file transfer</description>
- <field type="uint64_t" name="transfer_uid">Unique transfer
ID</field>
- <field type="char[240]" name="dest_path">Destination
path</field>
- <field type="uint8_t" name="direction">Transfer direction: 0:
from requester, 1: to requester</field>
- <field type="uint32_t" name="file_size">File size in
bytes</field>
- <field type="uint8_t" name="flags">RESERVED</field>
- </message>
- <message id="111" name="FILE_TRANSFER_DIR_LIST">
- <description>Get directory listing</description>
- <field type="uint64_t" name="transfer_uid">Unique transfer
ID</field>
- <field type="char[240]" name="dir_path">Directory path to
list</field>
- <field type="uint8_t" name="flags">RESERVED</field>
- </message>
- <message id="112" name="FILE_TRANSFER_RES">
- <description>File transfer result</description>
- <field type="uint64_t" name="transfer_uid">Unique transfer
ID</field>
- <field type="uint8_t" name="result">0: OK, 1: not permitted,
2: bad path / file name, 3: no space left on device</field>
- </message>
+ <message id="110" name="FILE_TRANSFER_PROTOCOL">
+ <description>File transfer message</description>
+ <field type="uint8_t" name="target_network">Network ID (0 for
broadcast)</field>
+ <field type="uint8_t" name="target_system">System ID (0 for
broadcast)</field>
+ <field type="uint8_t" name="target_component">Component ID (0
for broadcast)</field>
+ <field type="uint8_t[251]" name="payload">Variable length
payload. The length is defined by the remaining message length when
subtracting the header and other fields. The entire content of this block
is opaque unless you understand any the encoding message_type. The
particular encoding used can be extension specific and might not always be
documented as part of the mavlink specification.</field>
+ </message>
+ <message id="111" name="TIMESYNC">
+ <description>Time synchronization message.</description>
+ <field type="int64_t" name="tc1">Time sync timestamp 1</field>
+ <field type="int64_t" name="ts1">Time sync timestamp 2</field>
+ </message>
<message id="113" name="HIL_GPS">
<description>The global position, as returned by the Global
Positioning System (GPS). This is
NOT the global position estimate of the sytem, but rather
a RAW sensor value. See message GLOBAL_POSITION for the global position
estimate. Coordinate frame is right-handed, Z-axis up (GPS
frame).</description>
@@ -2099,20 +2284,24 @@
<field type="uint8_t" name="satellites_visible">Number of
satellites visible. If unknown, set to 255</field>
</message>
<message id="114" name="HIL_OPTICAL_FLOW">
- <description>Simulated optical flow from a flow sensor (e.g.
optical mouse sensor)</description>
- <field type="uint64_t" name="time_usec">Timestamp
(UNIX)</field>
+ <description>Simulated optical flow from a flow sensor (e.g.
PX4FLOW or optical mouse sensor)</description>
+ <field type="uint64_t" name="time_usec">Timestamp
(microseconds, synced to UNIX time or since system boot)</field>
<field type="uint8_t" name="sensor_id">Sensor ID</field>
- <field type="int16_t" name="flow_x">Flow in pixels in
x-sensor direction</field>
- <field type="int16_t" name="flow_y">Flow in pixels in
y-sensor direction</field>
- <field type="float" name="flow_comp_m_x">Flow in meters in
x-sensor direction, angular-speed compensated</field>
- <field type="float" name="flow_comp_m_y">Flow in meters in
y-sensor direction, angular-speed compensated</field>
- <field type="uint8_t" name="quality">Optical flow quality /
confidence. 0: bad, 255: maximum quality</field>
- <field type="float" name="ground_distance">Ground distance in
meters. Positive value: distance known. Negative value: Unknown
distance</field>
+ <field type="uint32_t" name="integration_time_us">Integration
time in microseconds. Divide integrated_x and integrated_y by the
integration time to obtain average flow. The integration time also
indicates the.</field>
+ <field type="float" name="integrated_x">Flow in radians
around X axis (Sensor RH rotation about the X axis induces a positive flow.
Sensor linear motion along the positive Y axis induces a negative
flow.)</field>
+ <field type="float" name="integrated_y">Flow in radians
around Y axis (Sensor RH rotation about the Y axis induces a positive flow.
Sensor linear motion along the positive X axis induces a positive
flow.)</field>
+ <field type="float" name="integrated_xgyro">RH rotation
around X axis (rad)</field>
+ <field type="float" name="integrated_ygyro">RH rotation
around Y axis (rad)</field>
+ <field type="float" name="integrated_zgyro">RH rotation
around Z axis (rad)</field>
+ <field type="int16_t" name="temperature">Temperature * 100 in
centi-degrees Celsius</field>
+ <field type="uint8_t" name="quality">Optical flow quality /
confidence. 0: no valid flow, 255: maximum quality</field>
+ <field type="uint32_t" name="time_delta_distance_us">Time in
microseconds since the distance was sampled.</field>
+ <field type="float" name="distance">Distance to the center of
the flow field in meters. Positive value (including zero): distance known.
Negative value: Unknown distance.</field>
</message>
<message id="115" name="HIL_STATE_QUATERNION">
<description>Sent from simulation to autopilot, avoids in
contrast to HIL_STATE singularities. This packet is useful for high
throughput applications such as hardware in the loop
simulations.</description>
<field type="uint64_t" name="time_usec">Timestamp
(microseconds since UNIX epoch or microseconds since system boot)</field>
- <field type="float[4]" name="attitude_quaternion">Vehicle
attitude expressed as normalized quaternion</field>
+ <field type="float[4]" name="attitude_quaternion">Vehicle
attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0
0 being the null-rotation)</field>
<field type="float" name="rollspeed">Body frame roll / phi
angular speed (rad/s)</field>
<field type="float" name="pitchspeed">Body frame pitch /
theta angular speed (rad/s)</field>
<field type="float" name="yawspeed">Body frame yaw / psi
angular speed (rad/s)</field>
@@ -2181,7 +2370,7 @@
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component
ID</field>
</message>
- <message name="GPS_INJECT_DATA" id="123">
+ <message name="GPS_INJECT_DATA" id="123">
<description>data for injecting into the onboard GPS (used for
DGPS)</description>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component
ID</field>
@@ -2249,20 +2438,20 @@
<field type="int32_t" name="baseline_c_mm">Current baseline in
ECEF z or NED down component in mm.</field>
<field type="uint32_t" name="accuracy">Current estimate of
baseline accuracy.</field>
<field type="int32_t" name="iar_num_hypotheses">Current number
of integer ambiguity hypotheses.</field>
- </message>
- <message id="130" name="DATA_TRANSMISSION_HANDSHAKE">
- <field type="uint8_t" name="type">type of
requested/acknowledged data (as defined in ENUM DATA_TYPES in
mavlink/include/mavlink_types.h)</field>
- <field type="uint32_t" name="size">total data size in bytes
(set on ACK only)</field>
- <field type="uint16_t" name="width">Width of a matrix or
image</field>
- <field type="uint16_t" name="height">Height of a matrix or
image</field>
- <field type="uint16_t" name="packets">number of packets
beeing sent (set on ACK only)</field>
- <field type="uint8_t" name="payload">payload size per packet
(normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set
on ACK only)</field>
- <field type="uint8_t" name="jpg_quality">JPEG quality out of
[1,100]</field>
- </message>
- <message id="131" name="ENCAPSULATED_DATA">
- <field type="uint16_t" name="seqnr">sequence number (starting
with 0 on every transmission)</field>
- <field type="uint8_t[253]" name="data">image data
bytes</field>
- </message>
+ </message>
+ <message id="130" name="DATA_TRANSMISSION_HANDSHAKE">
+ <field type="uint8_t" name="type">type of
requested/acknowledged data (as defined in ENUM DATA_TYPES in
mavlink/include/mavlink_types.h)</field>
+ <field type="uint32_t" name="size">total data size in bytes
(set on ACK only)</field>
+ <field type="uint16_t" name="width">Width of a matrix or
image</field>
+ <field type="uint16_t" name="height">Height of a matrix or
image</field>
+ <field type="uint16_t" name="packets">number of packets beeing
sent (set on ACK only)</field>
+ <field type="uint8_t" name="payload">payload size per packet
(normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set
on ACK only)</field>
+ <field type="uint8_t" name="jpg_quality">JPEG quality out of
[1,100]</field>
+ </message>
+ <message id="131" name="ENCAPSULATED_DATA">
+ <field type="uint16_t" name="seqnr">sequence number (starting
with 0 on every transmission)</field>
+ <field type="uint8_t[253]" name="data">image data bytes</field>
+ </message>
<message id="132" name="DISTANCE_SENSOR">
<field type="uint32_t" name="time_boot_ms">Time since system
boot</field>
<field type="uint16_t" name="min_distance">Minimum distance
the sensor can measure in centimeters</field>
@@ -2273,43 +2462,63 @@
<field type="uint8_t" name="orientation">Direction the sensor
faces from FIXME enum.</field>
<field type="uint8_t" name="covariance">Measurement covariance
in centimeters, 0 for unknown / invalid readings</field>
</message>
- <message id="147" name="BATTERY_STATUS">
- <description>Transmitte battery informations for a accu
pack.</description>
- <field type="uint8_t" name="accu_id">Accupack ID</field>
- <field type="uint16_t" name="voltage_cell_1">Battery
voltage of cell 1, in millivolts (1 = 1 millivolt)</field>
- <field type="uint16_t" name="voltage_cell_2">Battery
voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell</field>
- <field type="uint16_t" name="voltage_cell_3">Battery
voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell</field>
- <field type="uint16_t" name="voltage_cell_4">Battery
voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell</field>
- <field type="uint16_t" name="voltage_cell_5">Battery
voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell</field>
- <field type="uint16_t" name="voltage_cell_6">Battery
voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell</field>
- <field type="int16_t" name="current_battery">Battery
current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not
measure the current</field>
- <field type="int32_t" name="current_consumed">Consumed
charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide
mAh consumption estimate</field>
- <field type="int32_t" name="energy_consumed">Consumed
energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot
does not provide energy consumption estimate</field>
- <field type="int8_t" name="battery_remaining">Remaining
battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the
remaining battery</field>
- </message>
- <message id="148" name="SETPOINT_8DOF">
- <description>Set the 8 DOF setpoint for a
controller.</description>
- <field type="uint8_t" name="target_system">System ID</field>
- <field type="float" name="val1">Value 1</field>
- <field type="float" name="val2">Value 2</field>
- <field type="float" name="val3">Value 3</field>
- <field type="float" name="val4">Value 4</field>
- <field type="float" name="val5">Value 5</field>
- <field type="float" name="val6">Value 6</field>
- <field type="float" name="val7">Value 7</field>
- <field type="float" name="val8">Value 8</field>
- </message>
- <message id="149" name="SETPOINT_6DOF">
- <description>Set the 6 DOF setpoint for a attitude and
position controller.</description>
- <field type="uint8_t" name="target_system">System ID</field>
- <field type="float" name="trans_x">Translational Component
in x</field>
- <field type="float" name="trans_y">Translational Component
in y</field>
- <field type="float" name="trans_z">Translational Component
in z</field>
- <field type="float" name="rot_x">Rotational Component in
x</field>
- <field type="float" name="rot_y">Rotational Component in
y</field>
- <field type="float" name="rot_z">Rotational Component in
z</field>
- </message>
- <!-- MESSAGE IDs 150 - 240: Space for custom messages in
individual projectname_messages.xml files -->
+ <message id="133" name="TERRAIN_REQUEST">
+ <description>Request for terrain data and terrain
status</description>
+ <field type="int32_t" name="lat">Latitude of SW corner of
first grid (degrees *10^7)</field>
+ <field type="int32_t" name="lon">Longitude of SW corner of
first grid (in degrees *10^7)</field>
+ <field type="uint16_t" name="grid_spacing">Grid spacing in
meters</field>
+ <field type="uint64_t" name="mask"
print_format="0x%07x">Bitmask of requested 4x4 grids (row major 8x7 array
of grids, 56 bits)</field>
+ </message>
+ <message id="134" name="TERRAIN_DATA">
+ <description>Terrain data sent from GCS. The lat/lon and
grid_spacing must be the same as a lat/lon from a
TERRAIN_REQUEST</description>
+ <field type="int32_t" name="lat">Latitude of SW corner of
first grid (degrees *10^7)</field>
+ <field type="int32_t" name="lon">Longitude of SW corner of
first grid (in degrees *10^7)</field>
+ <field type="uint16_t" name="grid_spacing">Grid spacing in
meters</field>
+ <field type="uint8_t" name="gridbit">bit within the terrain
request mask</field>
+ <field type="int16_t[16]" name="data">Terrain data in meters
AMSL</field>
+ </message>
+ <message id="135" name="TERRAIN_CHECK">
+ <description>Request that the vehicle report terrain height at
the given location. Used by GCS to check if vehicle has all terrain data
needed for a mission.</description>
+ <field type="int32_t" name="lat">Latitude (degrees
*10^7)</field>
+ <field type="int32_t" name="lon">Longitude (degrees
*10^7)</field>
+ </message>
+ <message id="136" name="TERRAIN_REPORT">
+ <description>Response from a TERRAIN_CHECK
request</description>
+ <field type="int32_t" name="lat">Latitude (degrees
*10^7)</field>
+ <field type="int32_t" name="lon">Longitude (degrees
*10^7)</field>
+ <field type="uint16_t" name="spacing">grid spacing (zero if
terrain at this location unavailable)</field>
+ <field type="float" name="terrain_height">Terrain height in
meters AMSL</field>
+ <field type="float" name="current_height">Current vehicle
height above lat/lon terrain height (meters)</field>
+ <field type="uint16_t" name="pending">Number of 4x4 terrain
blocks waiting to be received or read from disk</field>
+ <field type="uint16_t" name="loaded">Number of 4x4 terrain
blocks in memory</field>
+ </message>
+ <message id="147" name="BATTERY_STATUS">
+ <description>Battery information</description>
+ <field type="uint8_t" name="id">Battery ID</field>
+ <field type="uint8_t" name="battery_function"
enum="MAV_BATTERY_FUNCTION">Function of the battery</field>
+ <field type="uint8_t" name="type" enum="MAV_BATTERY_TYPE">Type
(chemistry) of the battery</field>
+ <field type="int16_t" name="temperature">Temperature of the
battery in centi-degrees celsius. INT16_MAX for unknown temperature.</field>
+ <field type="uint16_t[10]" name="voltages">Battery voltage of
cells, in millivolts (1 = 1 millivolt)</field>
+ <field type="int16_t" name="current_battery">Battery current,
in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the
current</field>
+ <field type="int32_t" name="current_consumed">Consumed charge,
in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh
consumption estimate</field>
+ <field type="int32_t" name="energy_consumed">Consumed energy,
in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not
provide energy consumption estimate</field>
+ <field type="int8_t" name="battery_remaining">Remaining
battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the
remaining battery</field>
+ </message>
+ <message id="148" name="AUTOPILOT_VERSION">
+ <description>Version and capability of autopilot
software</description>
+ <field type="uint64_t" name="capabilities">bitmask of
capabilities (see MAV_PROTOCOL_CAPABILITY enum)</field>
+ <field type="uint32_t" name="version">Firmware version
number</field>
+ <field type="uint8_t[8]" name="custom_version">Custom version
field, commonly the first 8 bytes (16 characters) of the git hash. This is
not an unique identifier, but should allow to identify the commit using the
main version number even for very large code bases.</field>
+ </message>
+ <!-- MESSAGE IDs 180 - 240: Space for custom messages in
individual projectname_messages.xml files -->
+ <message id="248" name="V2_EXTENSION">
+ <description>Message implementing parts of the V2 payload
specs in V1 frames for transitional support.</description>
+ <field type="uint8_t" name="target_network">Network ID (0 for
broadcast)</field>
+ <field type="uint8_t" name="target_system">System ID (0 for
broadcast)</field>
+ <field type="uint8_t" name="target_component">Component ID (0
for broadcast)</field>
+ <field type="uint16_t" name="message_type">A code that
identifies the software component that understands this message (analogous
to usb device classes or mime type strings). If this code is less than
32768, it is considered a 'registered' protocol extension and the
corresponding entry should be added to
https://github.com/mavlink/mavlink/extension-message-ids.xml. Software
creators can register blocks of message IDs as needed (useful for GCS
specific metadata, etc...). Message_types greater than 32767 are considered
local experiments and should not be checked in to any widely distributed
codebase.</field>
+ <field type="uint8_t[249]" name="payload">Variable length
payload. The length is defined by the remaining message length when
subtracting the header and other fields. The entire content of this block
is opaque unless you understand any the encoding message_type. The
particular encoding used can be extension specific and might not always be
documented as part of the mavlink specification.</field>
+ </message>
<message id="249" name="MEMORY_VECT">
<description>Send raw controller memory. The use of this
message is discouraged for normal packets, but a quite efficient way for
testing new messages and getting experimental debug output.</description>
<field type="uint16_t" name="address">Starting address of
the debug variables</field>
=======================================
--- /trunk/Tools/MatrixPilot-SIL/SIL-udb.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/Tools/MatrixPilot-SIL/SIL-udb.c Sat Aug 1 04:50:37 2015 UTC
@@ -291,21 +291,8 @@
// not simulated
(void)newValue; // unused parameter
}
-/*
-Team,
-I figured out what is going on with offsets in HILSIM/SILSIM....I am sorry
I did not spot it sooner....
-Offsets are not used in HILSIM/SILSIM, since they are not needed. So it
does not matter how they are computed.
-We really did not need to compute them. The actual values of the offsets
are bogus.
-Sorry for all the mud I may have stirred up.
-Best regards,
-Bill
- */
+
void udb_a2d_record_offsets(void)
-{
-}
-/*
-#ifdef INITIALIZE_VERTICAL // for VTOL, vertical initialization
-void udb_a2d_record_offsets(void)
{
// almost ready to turn the control on, save the input offsets
UDB_XACCEL.offset = UDB_XACCEL.value;
@@ -316,24 +303,6 @@
udb_zrate.offset = udb_zrate.value;
udb_vref.offset = udb_vref.value;
}
-#else // horizontal initialization
-void udb_a2d_record_offsets(void)
-{
- // almost ready to turn the control on, save the input offsets
- UDB_XACCEL.offset = UDB_XACCEL.value;
- udb_xrate.offset = udb_xrate.value;
- UDB_YACCEL.offset = UDB_YACCEL.value;
- udb_yrate.offset = udb_yrate.value;
- UDB_ZACCEL.offset = UDB_ZACCEL.value + (Z_GRAVITY_SIGN
((int16_t)(2*GRAVITY))); // same direction
- udb_zrate.offset = udb_zrate.value;
- udb_vref.offset = udb_vref.value;
-
- DPRINT("udb_a2d_record_offsets() %u, %u, %u, %u, %u, %u\r\n",
- UDB_XACCEL.offset, udb_xrate.offset, UDB_YACCEL.offset, udb_yrate.offset,
- UDB_ZACCEL.offset, udb_zrate.offset);
-}
-#endif // INITIALIZE_VERTICAL
- */

uint16_t get_reset_flags(void)
{
=======================================
--- /trunk/Tools/MatrixPilot-SIL/module.mk Tue Mar 3 04:00:39 2015 UTC
+++ /trunk/Tools/MatrixPilot-SIL/module.mk Sat Aug 1 04:50:37 2015 UTC
@@ -2,9 +2,4 @@

local_src := $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.c)

-ifneq (,$(filter $(TOOLCHAIN), C30 XC16))
-local_src += $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.s)
-else
-endif
-
$(eval $(call make-target,$(subdirectory)/$(subdirectory).a,$(local_src)))
=======================================
--- /trunk/Tools/flight_analyzer/check_telemetry_type.py Fri May 3
19:48:17 2013 UTC
+++ /trunk/Tools/flight_analyzer/check_telemetry_type.py Sat Aug 1
04:50:37 2015 UTC
@@ -20,10 +20,10 @@


try:
- sys.path.insert(0,
os.path.join(os.getcwd(), '..', 'MAVLink', 'mavlink', 'pymavlink'))
+ sys.path.insert(0,
os.path.join(os.getcwd(), '..', 'MAVLink', 'mavlink'))
os.environ['MAVLINK10'] = '1'
- import mavlinkv10 as mavlink
- import mavutil
+ import pymavlink.dialects.v10.matrixpilot as mavlink
+ import pymavlink.mavutil as mavutil
except:
print "Not able to find Python MAVlink libraries"

@@ -85,7 +85,7 @@
if (parsing_index + payload_length + 7) < len(bytes) :
mycrc.accumulate(bytes[(parsing_index + 1 ):(parsing_index
+ payload_length + 6)])
msgId = bytes[parsing_index + 5]
- (fmt, type, order_map, crc_extra) =
mavlink.mavlink_map[msgId]
+ (fmt, type, order_map, len_map, crc_extra) =
mavlink.mavlink_map[msgId]
mycrc.accumulate(struct.pack('B',crc_extra))

low_byte_sent_crc = bytes[parsing_index + payload_length +
6]
=======================================
--- /trunk/Tools/flight_analyzer/matrixpilot_lib.py Sun Jan 5 18:30:00
2014 UTC
+++ /trunk/Tools/flight_analyzer/matrixpilot_lib.py Sat Aug 1 04:50:37
2015 UTC
@@ -1,13 +1,15 @@
+# -*- coding: utf-8 -*-
import re
import sys
import os
+import math


try:
- sys.path.insert(0,
os.path.join(os.getcwd(), '..', 'MAVLink', 'mavlink', 'pymavlink'))
+ sys.path.insert(0,
os.path.join(os.getcwd(), '..', 'MAVLink', 'mavlink'))
os.environ['MAVLINK10'] = '1'
- import mavlinkv10 as mavlink
- import mavutil
+ import pymavlink.dialects.v10.matrixpilot as mavlink
+ import pymavlink.mavutil as mavutil
except:
print "Not able to find Python MAVlink libraries"

@@ -19,21 +21,25 @@
"""Model a mavlink file (one without local time stamps inserted)"""
def __init__(self, filename, type_of_mavlink):
if (type_of_mavlink == "SERIAL_MAVLINK_RAW"):
- self.m = mavutil.mavlink_connection(filename, notimestamps =
True)
+ self.m = mavutil.mavlink_connection(filename, notimestamps =
True,dialect='matrixpilot')
elif (type_of_mavlink == "SERIAL_MAVLINK_TIMESTAMPS"):
- self.m = mavutil.mavlink_connection(filename, notimestamps =
False)
+ self.m = mavutil.mavlink_connection(filename, notimestamps =
False, dialect='matrixpilot')
else:
print "Error: Unknown Mavlink file type (not raw or
timestamp)."
self.total_mavlink_packets_received = 0
self.dropped_mavlink_packets = 0
self.first_packet_flag = True
self.SUE_F2_A_needs_printing = False
+ self.last_aero_force_x = 0
+ self.last_aero_force_y = 0
+ self.last_aero_force_z = 0
+

def __iter__(self):
return(self)

def next(self):
- """return the next good SERIAL UDB EXTRA (SUE) Binary MAVLink
record"""
+ """return the next good Binary MAVLink record of interest (e.g.
SUE)"""
while True :
self.msg = self.m.recv_match(blocking=False)
if not self.msg:
@@ -42,15 +48,18 @@
print "Packets dropped in transmission:",
self.dropped_mavlink_packets
raise StopIteration
elif self.msg.get_type() == "BAD_DATA":
+ print "Bad Data Packet"
pass
else : # We have a good mavlink packet
self.track_dropped_packets(self.msg.get_seq())
- #print self.msg.get_seq()
+ #print self.msg
if self.msg.get_type() == "SERIAL_UDB_EXTRA_F2_A":
+ #print self.msg
self.last_F2_A_msg = self.msg
self.SUE_F2_A_needs_printing = True
continue
elif self.msg.get_type() == "SERIAL_UDB_EXTRA_F2_B":
+ #print self.msg
try:
if self.msg.sue_time >=
(self.last_F2_A_msg.sue_time - 250) : #A and B halves of message are a pair
# Note for above: -250 is because time can
go backwards by 1/4 of a second in MP when GPS updates.
@@ -74,7 +83,13 @@
self.msg.get_type() == 'SERIAL_UDB_EXTRA_F15' or \
self.msg.get_type() == 'SERIAL_UDB_EXTRA_F17':
#print self.msg.get_seq(),"DEBUG: ",
self.msg.get_type()
- return self.msg
+ return self.msg
+ elif self.msg.get_type() == 'FORCE':
+ self.last_aero_force_x = self.msg.aero_x # hang
onto message until we process F2_B message later
+ self.last_aero_force_y = self.msg.aero_y
+ self.last_aero_force_z = self.msg.aero_z
+ #print self.msg
+ continue
else :
#print "Ignoring non SUE MAVLink message",
self.msg.get_type()
pass
@@ -220,6 +235,29 @@
self.flags = 0
self.sonar_direct = 0 # Direct distance in cm to sonar target
self.alt_sonar = 0 # Calculated altitude above ground of plane
in cm
+ # The following variables added for the Helical Turns method of
fly by wire and auto flight
+ self.aero_force_x = 0
+ self.aero_force_y = 0
+ self.aero_force_z = 0
+ self.feed_forward = 0
+ self.navigation_max_earth_vertical_axis_rotation_rate = 0
+ self.fly_by_wire_max_earth_vertical_axis_rotation_rate = 0
+ self.angle_of_attack_normal = 0
+ self.angle_of_attack_inverted = 0
+ self.elevator_trim_normal = 0
+ self.elevator_trim_inverted = 0
+ self.nominal_cruise_speed = 0
+ # End of new variables for Helical Turns
+ self.aileron_output_channel = 0
+ self.elevator_output_channel = 0
+ self.throttle_output_channel = 0
+ self.rudder_output_channel = 0
+ self.aileron_output_reversed = 0
+ self.elevator_output_reversed = 0
+ self.throttle_output_reversed = 0
+ self.rudder_output_reversed = 0
+ self.number_of_input_channels = 0
+ self.channel_trim_values = [0]


class mavlink_telemetry(base_telemetry):
@@ -273,6 +311,12 @@
self.svs = int(telemetry_file.last_F2_A_msg.sue_svs)
self.hdop = int(telemetry_file.last_F2_A_msg.sue_hdop)

+ # Storing the last aero force message received before
+ # the F2 SUE message, with the F2 Sue message.
+ self.aero_force_x = int(telemetry_file.last_aero_force_x)
+ self.aero_force_y = int(telemetry_file.last_aero_force_y)
+ self.aero_force_z = int(telemetry_file.last_aero_force_z)
+
self.pwm_input[1] = int(telemetry_file.msg.sue_pwm_input_1)
self.pwm_input[2] = int(telemetry_file.msg.sue_pwm_input_2)
self.pwm_input[3] = int(telemetry_file.msg.sue_pwm_input_3)
@@ -298,12 +342,12 @@
self.IMUlocationx_W1 =
int(telemetry_file.msg.sue_imu_location_x)
self.IMUlocationy_W1 =
int(telemetry_file.msg.sue_imu_location_y)
self.IMUlocationz_W1 =
int(telemetry_file.msg.sue_imu_location_z)
- self.sue_flags = int(telemetry_file.msg.sue_flags)
+ self.flags = int(telemetry_file.msg.sue_flags)

- self.sue_osc_fails = int(telemetry_file.msg.sue_osc_fails)
- self.sue_imu_velocity_x =
int(telemetry_file.msg.sue_imu_velocity_x)
- self.sue_imu_velocity_y =
int(telemetry_file.msg.sue_imu_velocity_y)
- self.sue_imu_velocity_z =
int(telemetry_file.msg.sue_imu_velocity_z)
+ self.osc_fails = int(telemetry_file.msg.sue_osc_fails)
+ self.IMUvelocityx =
int(telemetry_file.msg.sue_imu_velocity_x)
+ self.IMUvelocityy =
int(telemetry_file.msg.sue_imu_velocity_y)
+ self.IMUvelocityz =
int(telemetry_file.msg.sue_imu_velocity_z)
self.inline_waypoint_x =
int(telemetry_file.msg.sue_waypoint_goal_x)
self.inline_waypoint_y =
int(telemetry_file.msg.sue_waypoint_goal_y)
self.inline_waypoint_z =
int(telemetry_file.msg.sue_waypoint_goal_z)
@@ -402,7 +446,7 @@
self.id_diy_drones_url =
telemetry_file.msg.sue_ID_DIY_DRONES_URL

return("F16")
-
+
else :
err_message = "Warn:" + telemetry_file.msg.get_type()
return(err_message)
@@ -1146,7 +1190,6 @@
return "Error"
else :
pass
-
match = re.match(".*:fgs([-0-9]*?):",line) # flags from
defines.h
if match :
try:
@@ -1175,6 +1218,16 @@
except:
print "Corrupt F2: sonar value in line", line_no
pass
+ match =
re.match(".*:AF([-0-9]*?),([-0-9]*?),([-0-9]*?):",line) # Next waypoint
X,Y,Z in meters from origin
+ if match :
+ try:
+ self.aero_force_x = int(match.group(1))
+ self.aero_force_y = int(match.group(2))
+ self.aero_force_z = int(match.group(3))
+ except:
+ print "Corrupt F2: Aero Force value in line", line_no
+ pass
+

# line was parsed without major errors
return "F2"
@@ -1285,7 +1338,7 @@

match = re.match(".*:A_BOOST=(.*?):",line) # AILERON_BOOST
if match :
- self.aileron_boost = float(match.group(1))
+ self.aileron_boost = match.group(1)
else :
print "Failure parsing AILERON_BOOST at line", line_no
return "Error"
@@ -1314,14 +1367,14 @@
return "Error"
match = re.match(".*:RUD_E_MIX=(.*?):",line) # RUDDER_ELEV_MIX
if match :
- self.rudder_elev_mix = float(match.group(1))
+ self.rudder_elev_mix = match.group(1)
else :
print "Failure parsing RUDDER_ELEV_MIX at line", line_no
return "Error"

match = re.match(".*:ROL_E_MIX=(.*?):",line) # ROLL_ELEV_MIX
if match :
- self.roll_elev_mix = float (match.group(1))
+ self.roll_elev_mix = match.group(1)
else :
print "Failure parsing ROLL_ELEV_MIX at line", line_no
return "Error"
@@ -1643,10 +1696,116 @@
else :
print "Failure parsing ID_DIY_DRONES_URL at line", line_no
return "F16"
-
+
+ #################################################################
+ # Try Another format of telemetry
+
+ match = re.match("^F17:",line) # If line starts with F17
+ if match :
+ # Parse the line for options.h values
+
+ match = re.match(".*:FD_FWD=(.*?):",line) # FEED FORWARD for
Helical Turns
+ if match :
+ self.feed_forward = float(match.group(1))
+ else :
+ print "Failure parsing FEED FORWARD at line", line_no
+ match = re.match(".*:TR_NAV=(.*?):",line) # TR_NAV
+ if match :
+ self.navigation_max_earth_vertical_axis_rotation_rate =
float(match.group(1))
+ else :
+ print "Failure parsing TURN_RATE_NAV at line", line_no
+ match = re.match(".*:TR_FBW=(.*?):",line) # TR_FBW
+ if match :
+ self.fly_by_wire_max_earth_vertical_axis_rotation_rate =
float(match.group(1))
+ else :
+ print "Failure parsing TURN_RATE_FBW at line", line_no
+ return "F17"
+
#################################################################
# Try Another format of telemetry

+ match = re.match("^F18:",line) # If line starts with F18
+ if match :
+ # Parse the line for options.h values
+
+ match = re.match(".*:AOA_NRM=(.*?):",line) #
ANGLE_OF_ATTACK_NORMAL
+ if match :
+ self.angle_of_attack_normal = float(match.group(1))
+ else :
+ print "Failure parsing ANGLE_OF_ATTACK_NORMAL at line",
line_no
+ match = re.match(".*:AOA_INV=(.*?):",line) #
ANGLE_OF_ATTACK_INVERTED
+ if match :
+ self.angle_of_attack_inverted = float(match.group(1))
+ else :
+ print "Failure parsing ANGLE_OF_ATTACK_INVERTED at line",
line_no
+ match = re.match(".*:EL_TRIM_NRM=(.*?):",line) #
ELEVTOR_TRIM_NORMAL
+ if match :
+ self.elevator_trim_normal = float(match.group(1))
+ else :
+ print "Failure parsing ELEVATOR_TRIM_NORMAL at line",
line_no
+ match = re.match(".*:EL_TRIM_INV=(.*?):",line) #
ELEVTOR_TRIM_INVERTED
+ if match :
+ self.elevator_trim_inverted = float(match.group(1))
+ else :
+ print "Failure parsing ELEVATOR_TRIM_INVERTED at line",
line_no
+ match = re.match(".*:CRUISE_SPD=(.*?):",line) # Nominal
CRUISE_SPEED
+ if match :
+ self.nominal_cruise_speed = float(match.group(1))
+ else :
+ print "Failure parsing nominal CRUISE_SPEED at line",
line_no
+ return "F18"
+
+ #################################################################
+ # Try Another format of telemetry
+
+ match = re.match("^F19:",line) # If line starts with F19
+ if match :
+ # Parse the line for options.h values
+ match = re.match(".*:AIL=([0-9]*?),([0-9]*?):",line) #
Aileron Channel, and reversal
+ if match :
+ self.aileron_output_channel = int(match.group(1))
+ self.aileron_output_reversed = int(match.group(2))
+ else :
+ print "Failure parsing AILERON CHANNEL at line", line_no
+ match = re.match(".*:ELEV=([0-9]*?),([0-9]*?):",line) #
Elevator Channel, and reversal
+ if match :
+ self.elevator_output_channel = int(match.group(1))
+ self.elevator_output_reversed = int(match.group(2))
+ else :
+ print "Failure parsing ELEVATOR CHANNEL at line", line_no
+ match = re.match(".*:THROT=([0-9]*?),([0-9]*?):",line) #
Throttle Channel, and reversal
+ if match :
+ self.throttle_output_channel = int(match.group(1))
+ self.throttle_output_reversed = int(match.group(2))
+ else :
+ print "Failure parsing THROTTLE CHANNEL at line", line_no
+ match = re.match(".*:RUDD=([0-9]*?),([0-9]*?):",line) # Rudder
Channel, and reversal
+ if match :
+ self.rudder_output_channel = int(match.group(1))
+ self.rudder_output_reversed = int(match.group(2))
+ else :
+ print "Failure parsing RUDDER CHANNEL at line", line_no
+ return "F19"
+
+ #################################################################
+ # Try Another format of telemetry
+ match = re.match("^F20:",line) # If line starts with F20
+ if match :
+ # Parse the line for options.h values
+ match = re.match(".*:NUM_IN=([0-9]*?):",line) # Number of
Input Channels
+ if match :
+ self.number_of_input_channels = int(match.group(1))
+ else :
+ print "Failure parsing NUMBER OF INPUT CHANNELS at line",
line_no
+ # Only Trim values will match in the follwoing iterative line.
Here is an example:
+ # F20:NUM_IN=5:TRIM=2992,3000,2057,2989,2043,:
+ for match in re.finditer('([0-9]*?),',line):
+ self.channel_trim_values.append(int(match.group(1)))
+ return "F20"
+
+ #################################################################
+ # Try Another format of telemetry
+

match = re.match("^<tm>",line)
if match :
@@ -1869,6 +2028,32 @@
b[8] = a[8]
return b

+def normalize_vector_3x1(a) :
+ """ Make an arbitary 3x1 vector to have a unit length of 1"""
+ scalar_length = math.sqrt( a[0]**2 + a[1]**2 + a[2]**2 )
+ if scalar_length > 0 :
+ a[0] = a[0] / scalar_length
+ a[1] = a[1] / scalar_length
+ a[2] = a[2] / scalar_length
+ else :
+ a[0] = 0
+ a[1] = 0
+ a[2] = 0
+ return(a)
+
+def matrix_dot_product_vector_3x1(a,b) :
+ """ Perform a dot product operation on two vectors of dimensions 3x1"""
+ return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2])
+
+def matrix_cross_product_vector_3x1(a,b) :
+ """ Performa a cross product operation on two vectors of dimensions
3x1"""
+ # a × b = a2b3 − a3b2, a3b1 − a1b3, a1b2 − a2b1
+ c = [0,0,0]
+ c[0] = (a[1]*b[2]) - (a[2]*b[1])
+ c[1] = (a[2]*b[0]) - (a[0]*b[2])
+ c[2] = (a[0]*b[1]) - (a[1]*b[0])
+ return(c)
+
def write_mavlink_to_serial_udb_extra(telemetry_filename,
serial_udb_extra_filename, telemetry_type):
"""Filter out all the SERIAL_UDB_EXTRA messages from a MAVLink log
file, and print them to
to a ascii log file."""
=======================================
--- /trunk/Tools/makefiles/device-UDB5.mk Mon Jun 15 14:49:11 2015 UTC
+++ /trunk/Tools/makefiles/device-UDB5.mk Sat Aug 1 04:50:37 2015 UTC
@@ -2,6 +2,7 @@
# note: the project generator is not make, and hence does not obey make
syntax
# lines are matched as tokens once only, so do not use repeated +=
#
+
TOOLCHAIN ?= XC16
TARGET_TYPE := hex
CPU := 33FJ256GP710A
=======================================
--- /trunk/Tools/pyparam/ParameterDatabase.xml Wed May 6 15:02:33 2015 UTC
+++ /trunk/Tools/pyparam/ParameterDatabase.xml Sat Aug 1 04:50:37 2015 UTC
@@ -1,796 +1,862 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="UTF-8"?>
+
<!-- New document created with EditiX at Tue Aug 09 20:53:50 CEST 2011 -->
-<ParameterDatabase xmlns="ParameterDB"
+
+
+<ParameterDatabase
+xmlns="ParameterDB"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="ParameterDB ParameterDatabase.xsd">
- <dataStorageAreas>
- <dataStorageArea>NULL</dataStorageArea>
- <dataStorageArea>FAILURE_DATALOG</dataStorageArea>
- <dataStorageArea>IMU_CALIB</dataStorageArea>
- <dataStorageArea>MAG_CALIB</dataStorageArea>
- <dataStorageArea>ANALOG_CALIB</dataStorageArea>
- <dataStorageArea>MIXER</dataStorageArea>
- <dataStorageArea>RTL_SETTINGS</dataStorageArea>
- <dataStorageArea>WAYPOINTS</dataStorageArea>
- <dataStorageArea>OSD_SETUP</dataStorageArea>
- <dataStorageArea>RADIO_TRIM</dataStorageArea>
- <dataStorageArea>CONTROL_GAINS</dataStorageArea>
- <dataStorageArea>THROTTLE_HEIGHT_OPTIONS</dataStorageArea>
- <dataStorageArea>AIRSPEED_OPTIONS</dataStorageArea>
- </dataStorageAreas>
- <serialisationFlags>
- <serialisationFlag>ALL</serialisationFlag>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- <serialisationFlag>SAVE_AFTER_CAL</serialisationFlag>
- <serialisationFlag>STORE_WAYPOINTS</serialisationFlag>
- <serialisationFlag>STORE_CALIB</serialisationFlag>
- <serialisationFlag>SAVE_MISSION</serialisationFlag>
- </serialisationFlags>
- <udbTypes>
- <udbType>
- <typeName>UDB_TYPE_INT</typeName>
- <sendFunction>mavlink_send_param_int16</sendFunction>
- <setFunction>mavlink_set_param_int16</setFunction>
- <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_Q14</typeName>
- <sendFunction>mavlink_send_param_Q14</sendFunction>
- <setFunction>mavlink_set_param_Q14</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_PWTRIM</typeName>
- <sendFunction>mavlink_send_param_pwtrim</sendFunction>
- <setFunction>mavlink_set_param_pwtrim</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_GYROSCALE_Q14</typeName>
- <sendFunction>mavlink_send_param_gyroscale_Q14</sendFunction>
- <setFunction>mavlink_set_param_gyroscale_Q14</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_INT_CIRCULAR</typeName>
- <sendFunction>mavlink_send_int_circular</sendFunction>
- <setFunction>mavlink_set_int_circular</setFunction>
- <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_CM_AIRSPEED_TO_DM</typeName>
- <sendFunction>mavlink_send_dm_airspeed_in_cm</sendFunction>
- <setFunction>mavlink_set_dm_airspeed_from_cm</setFunction>
- <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_M_AIRSPEED_TO_DM</typeName>
- <sendFunction>mavlink_send_dm_airspeed_in_m</sendFunction>
- <setFunction>mavlink_set_dm_airspeed_from_m</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_M_AIRSPEED_TO_CM</typeName>
- <sendFunction>mavlink_send_cm_airspeed_in_m</sendFunction>
- <setFunction>mavlink_set_cm_airspeed_from_m</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_FRAME_ANGLERATE</typeName>
- <sendFunction>mavlink_send_frame_anglerate</sendFunction>
- <setFunction>mavlink_set_frame_anglerate</setFunction>
- <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_DCM_ANGLE</typeName>
- <sendFunction>mavlink_send_dcm_angle</sendFunction>
- <setFunction>mavlink_set_dcm_angle</setFunction>
- <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
- </udbType>
- <udbType>
- <typeName>UDB_TYPE_FLOAT</typeName>
- <sendFunction>mavlink_send_param_float</sendFunction>
- <setFunction>mavlink_set_param_float</setFunction>
- <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
- </udbType>
- </udbTypes>
- <parameterBlocks>
- <parameterBlock>
- <blockName>PID_SETTINGS</blockName>
- <storage_area>CONTROL_GAINS</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- <serialisationFlag>STORE_CALIB</serialisationFlag>
- </serialisationFlags>
- <load_callback>NULL</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>PID_ROLLKP</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>rollkp</variable_name>
- <description>Roll proporational gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_ROLLKD</parameterName>
- <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
- <variable_name>rollkd</variable_name>
- <description>Roll differential gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_YAWKPAIL</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>yawkpail</variable_name>
- <description>yaw to aileron gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_YAWKDAIL</parameterName>
- <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
- <variable_name>yawkdail</variable_name>
- <description>yaw to aileron rate gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_PITCHGAIN</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>pitchgain</variable_name>
- <description>PItch gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_PITCHKD</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>pitchkd</variable_name>
- <description>Pitch rate gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_RUDELEVGAIN</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>rudderElevMixGain</variable_name>
- <description>Roll differential gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_ROLLKPRUD</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>rollkprud</variable_name>
- <description>Roll differential gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_YAWKPRUD</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>yawkprud</variable_name>
- <description>Yaw to rudder gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_YAWKDRUD</parameterName>
- <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
- <variable_name>yawkdrud</variable_name>
- <description>Yaw to rudder rate gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_ROLLKPRUD</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>rollkprud</variable_name>
- <description>Roll to rudder gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>PID_ROLLKDRUD</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>rollkdrud</variable_name>
- <description>Roll to rudder rate gain</description>
- <min>0.0</min>
- <max>0.5</max>
- <readonly>false</readonly>
- </parameter>
- </parameters>
- <description>PID Settings Block</description>
- </parameterBlock>
- <parameterBlock>
- <blockName>MAG_CALIB</blockName>
- <storage_area>MAG_CALIB</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- <serialisationFlag>STORE_CALIB</serialisationFlag>
- </serialisationFlags>
- <includes>
- <includeString>../libUDB/magnetometer.h</includeString>
- </includes>
- <load_callback>NULL</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>MAG_CAL_RAW0</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>rawMagCalib[0]</variable_name>
- <description>magnetometer calibration raw 0</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_CAL_RAW1</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>rawMagCalib[1]</variable_name>
- <description>magnetometer calibration raw 0</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_CAL_RAW2</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>rawMagCalib[2]</variable_name>
- <description>magnetometer calibration raw 0</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_GAIN0</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>magGain[0]</variable_name>
- <description>magnetometer gain 0</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_GAIN1</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>magGain[1]</variable_name>
- <description>magnetometer gain 1</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_GAIN2</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>magGain[2]</variable_name>
- <description>magnetometer gain 2</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_OFFSET0</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_magOffset[0]</variable_name>
- <description>magnetometer offset 0</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_OFFSET1</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_magOffset[1]</variable_name>
- <description>magnetometer offset 1</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_OFFSET2</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_magOffset[2]</variable_name>
- <description>magnetometer offset 2</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>MAG_DECLINATION</parameterName>
- <udb_param_type>UDB_TYPE_INT_CIRCULAR</udb_param_type>
- <variable_name>dcm_declination_angle.BB</variable_name>
- <description>Magnetic declination</description>
- <min>-180</min>
- <max>180</max>
- <readonly>false</readonly>
- </parameter>
- </parameters>
- <description>Magnetometer parameters</description>
- </parameterBlock>
- <parameterBlock>
- <blockName>TRIM_CALIB</blockName>
- <storage_area>RADIO_TRIM</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- <serialisationFlag>STORE_CALIB</serialisationFlag>
- </serialisationFlags>
- <load_callback>udb_skip_radio_trim</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>PWTRIM_AILERON</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[AILERON_INPUT_CHANNEL]</variable_name>
- <description>RC aileron trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_ELEVATOR</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL]</variable_name>
- <description>RC aileron trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_RUDDER</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[RUDDER_INPUT_CHANNEL]</variable_name>
- <description>RC rudder trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_AILERON2</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[AILERON_SECONDARY_INPUT_CHANNEL]</variable_name>
- <description>RC aileron secondary trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_ROLL</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[ROLL_INPUT_CHANNEL]</variable_name>
- <description>RC roll trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_PITCH</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[PITCH_INPUT_CHANNEL]</variable_name>
- <description>RC pitch trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_THROTTLE</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[THROTTLE_INPUT_CHANNEL]</variable_name>
- <description>Throttle input rc trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_YAW</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[YAW_INPUT_CHANNEL]</variable_name>
- <description>RC yaw trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_FLAP</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[FLAP_INPUT_CHANNEL]</variable_name>
- <description>RC flap input trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_AIRBRAKE</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[BRAKE_INPUT_CHANNEL]</variable_name>
- <description>RC airbrake input trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_SPOILER</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[SPOILER_INPUT_CHANNEL]</variable_name>
- <description>RC spoiler input trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_CAMBER</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[CAMBER_INPUT_CHANNEL]</variable_name>
- <description>RC camber input trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_CROW</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[CROW_INPUT_CHANNEL]</variable_name>
- <description>RC crow input trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_CAMPITCH</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[CAMERA_PITCH_INPUT_CHANNEL]</variable_name>
- <description>RC yaw trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>PWTRIM_CAM_YAW</parameterName>
- <udb_param_type>UDB_TYPE_PWTRIM</udb_param_type>
- <variable_name>
- udb_pwTrim[CAMERA_YAW_INPUT_CHANNEL]</variable_name>
- <description>RC yaw trim</description>
- <min>800.0</min>
- <max>2200.0</max>
- <readonly>true</readonly>
- </parameter>
- </parameters>
- <description>Radio input pulse width offset
- calibration</description>
- </parameterBlock>
- <parameterBlock>
- <blockName>IMU_CALIB</blockName>
- <storage_area>IMU_CALIB</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- <serialisationFlag>STORE_CALIB</serialisationFlag>
- </serialisationFlags>
- <includes>
- <includeString>../libUDB/ADchannel.h</includeString>
- </includes>
- <externs></externs>
- <load_callback>udb_skip_imu_calibration</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>IMU_XACCEL_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_xaccel.offset</variable_name>
- <description>X Accelerometer offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_YACCEL_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_yaccel.offset</variable_name>
- <description>Y Accelerometer offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_ZACCEL_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_zaccel.offset</variable_name>
- <description>Z Accelerometer offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_XGYRO_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_xrate.offset</variable_name>
- <description>X gyro rate offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_YGYRO_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_yrate.offset</variable_name>
- <description>Ygyro rate offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_ZGYRO_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_zrate.offset</variable_name>
- <description>Z gyro rate offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- <parameter>
- <parameterName>IMU_VREF_OFF</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>udb_vref.offset</variable_name>
- <description>Vref offset</description>
- <min>-32767</min>
- <max>32767</max>
- <readonly>true</readonly>
- </parameter>
- </parameters>
- <description>Radio input pulse width offset
- calibration</description>
- </parameterBlock>
- <parameterBlock>
- <blockName>THROTTLE_HEIGHT_OPTIONS</blockName>
- <storage_area>THROTTLE_HEIGHT_OPTIONS</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- </serialisationFlags>
- <includes>
- <includeString>altitudeCntrl.h</includeString>
- </includes>
- <load_callback>NULL</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>TH_H_TARGET_MIN</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>height_target_min</variable_name>
- <description>Minimum taget height during stabilized
- flight</description>
- <min>0</min>
- <max>5000</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_H_TARGET_MAX</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>height_target_max</variable_name>
- <description>Maximum taget height during stabilized
- flight</description>
- <min>0</min>
- <max>5000</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_H_MARGIN</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>height_margin</variable_name>
- <description>Height margin for linear
- control</description>
- <min>1</min>
- <max>500</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_T_HOLD_MIN</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>alt_hold_throttle_min</variable_name>
- <description>Minimum autopilot or stabilised
- throttle</description>
- <min>0</min>
- <max>1</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_T_HOLD_MAX</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>alt_hold_throttle_max</variable_name>
- <description>Maximum autopilot or stabilised
- throttle</description>
- <min>0</min>
- <max>1</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_P_HOLD_MIN</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>alt_hold_pitch_min</variable_name>
- <description>Altitude hold pitch minimum</description>
- <min>-89</min>
- <max>0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_P_HOLD_MAX</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>alt_hold_pitch_max</variable_name>
- <description>Altitude hold pitch maximum</description>
- <min>0</min>
- <max>89</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_P_HIGH</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>alt_hold_pitch_high</variable_name>
- <description>Pitch down when above maximum
- altitude</description>
- <min>0</min>
- <max>89</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>TH_P_RTL_DOWN</parameterName>
- <udb_param_type>UDB_TYPE_INT</udb_param_type>
- <variable_name>rtl_pitch_down</variable_name>
- <description>Return to launch pitch down</description>
- <min>0</min>
- <max>89</max>
- <readonly>false</readonly>
- </parameter>
- </parameters>
- <description>Throttle and height control gains and
- options</description>
- </parameterBlock>
- <parameterBlock>
- <blockName>AIRSPEED_OPTIONS</blockName>
- <storage_area>AIRSPEED_OPTIONS</storage_area>
- <serialisationFlags>
- <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
- <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
- </serialisationFlags>
- <includes>
- <includeString>airspeedCntrl.h</includeString>
- </includes>
- <load_callback>NULL</load_callback>
- <in_mavlink_parameters>true</in_mavlink_parameters>
- <parameters>
- <parameter>
- <parameterName>ASPD_DESIRED</parameterName>
- <udb_param_type>
- UDB_TYPE_M_AIRSPEED_TO_DM</udb_param_type>
- <variable_name>desiredSpeed</variable_name>
- <description>Desired airspeed in m/s</description>
- <min>0</min>
- <max>300.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_MIN_GSPD</parameterName>
- <udb_param_type>
- UDB_TYPE_M_AIRSPEED_TO_CM</udb_param_type>
- <variable_name>minimum_groundspeed</variable_name>
- <description>Minimum groundspeed</description>
- <min>0</min>
- <max>20000</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_MIN</parameterName>
- <udb_param_type>
- UDB_TYPE_M_AIRSPEED_TO_CM</udb_param_type>
- <variable_name>minimum_airspeed</variable_name>
- <description>Minimum airspeed</description>
- <min>0</min>
- <max>300.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_MAX</parameterName>
- <udb_param_type>
- UDB_TYPE_M_AIRSPEED_TO_CM</udb_param_type>
- <variable_name>maximum_airspeed</variable_name>
- <description>Maximum airspeed</description>
- <min>0</min>
- <max>300.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_CRUISE</parameterName>
- <udb_param_type>
- UDB_TYPE_M_AIRSPEED_TO_CM</udb_param_type>
- <variable_name>cruise_airspeed</variable_name>
- <description>Cruise airspeed in normal level flight at
- zero degrees</description>
- <min>0</min>
- <max>300.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_P_MIN_ASPD</parameterName>
- <udb_param_type>UDB_TYPE_DCM_ANGLE</udb_param_type>
- <variable_name>airspeed_pitch_min_aspd</variable_name>
- <description>Estimated pitch change for minimum airspeed
- during glide(degrees)</description>
- <min>-90</min>
- <max>90.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_P_MAX_ASPD</parameterName>
- <udb_param_type>UDB_TYPE_DCM_ANGLE</udb_param_type>
- <variable_name>airspeed_pitch_max_aspd</variable_name>
- <description>Estimated pitch change required for maximum
- airspeed during glide(degrees)</description>
- <min>-90</min>
- <max>90.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_P_RATE_LIM</parameterName>
- <udb_param_type>UDB_TYPE_FRAME_ANGLERATE</udb_param_type>
- <variable_name>airspeed_pitch_adjust_rate</variable_name>
- <description>Pitch rate per second (in integer
- degrees)</description>
- <min>1.0</min>
- <max>720.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_P_KI</parameterName>
- <udb_param_type>UDB_TYPE_Q14</udb_param_type>
- <variable_name>airspeed_pitch_ki</variable_name>
- <description>Airspeed error integration
- gain</description>
- <min>0.0</min>
- <max>1.0</max>
- <readonly>false</readonly>
- </parameter>
- <parameter>
- <parameterName>ASPD_P_KI_LIMIT</parameterName>
- <udb_param_type>UDB_TYPE_DCM_ANGLE</udb_param_type>
- <variable_name>airspeed_pitch_ki_limit</variable_name>
- <description>Airspeed error integration limit
- (degrees)</description>
- <min>0.0</min>
- <max>45.0</max>
- <readonly>false</readonly>
- </parameter>
- </parameters>
- <description>Airspeed options</description>
- </parameterBlock>
- </parameterBlocks>
+ <dataStorageAreas>
+ <dataStorageArea>NULL</dataStorageArea>
+ <dataStorageArea>FAILURE_DATALOG</dataStorageArea>
+ <dataStorageArea>IMU_CALIB</dataStorageArea>
+ <dataStorageArea>MAG_CALIB</dataStorageArea>
+ <dataStorageArea>ANALOG_CALIB</dataStorageArea>
+ <dataStorageArea>MIXER</dataStorageArea>
+ <dataStorageArea>RTL_SETTINGS</dataStorageArea>
+ <dataStorageArea>WAYPOINTS</dataStorageArea>
+ <dataStorageArea>OSD_SETUP</dataStorageArea>
+ <dataStorageArea>RADIO_TRIM</dataStorageArea>
+ <dataStorageArea>CONTROL_GAINS</dataStorageArea>
+ <dataStorageArea>THROTTLE_HEIGHT_OPTIONS</dataStorageArea>
+ <dataStorageArea>AIRSPEED_OPTIONS</dataStorageArea>
+ </dataStorageAreas>
+
+<serialisationFlags>
+ <serialisationFlag>ALL</serialisationFlag>
+ <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
+ <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
+ <serialisationFlag>SAVE_AFTER_CAL</serialisationFlag>
+ <serialisationFlag>STORE_WAYPOINTS</serialisationFlag>
+ <serialisationFlag>STORE_CALIB</serialisationFlag>
+ <serialisationFlag>SAVE_MISSION</serialisationFlag>
+</serialisationFlags>
+
+
+<udbTypes>
+ <udbType>
+ <typeName>UDB_TYPE_INT</typeName>
+ <sendFunction>mavlink_send_param_int16</sendFunction>
+ <setFunction>mavlink_set_param_int16</setFunction>
+ <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_Q14</typeName>
+ <sendFunction>mavlink_send_param_Q14</sendFunction>
+ <setFunction>mavlink_set_param_Q14</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_PWTRIM</typeName>
+ <sendFunction>mavlink_send_param_pwtrim</sendFunction>
+ <setFunction>mavlink_set_param_pwtrim</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_GYROSCALE_Q14</typeName>
+ <sendFunction>mavlink_send_param_gyroscale_Q14</sendFunction>
+ <setFunction>mavlink_set_param_gyroscale_Q14</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_INT_CIRCULAR</typeName>
+ <sendFunction>mavlink_send_int_circular</sendFunction>
+ <setFunction>mavlink_set_int_circular</setFunction>
+ <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_CM_AIRSPEED_TO_DM</typeName>
+ <sendFunction>mavlink_send_dm_airspeed_in_cm</sendFunction>
+ <setFunction>mavlink_set_dm_airspeed_from_cm</setFunction>
+ <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_M_AIRSPEED_TO_DM</typeName>
+ <sendFunction>mavlink_send_dm_airspeed_in_m</sendFunction>
+ <setFunction>mavlink_set_dm_airspeed_from_m</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_M_AIRSPEED_TO_CM</typeName>
+ <sendFunction>mavlink_send_cm_airspeed_in_m</sendFunction>
+ <setFunction>mavlink_set_cm_airspeed_from_m</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_FRAME_ANGLERATE</typeName>
+ <sendFunction>mavlink_send_frame_anglerate</sendFunction>
+ <setFunction>mavlink_set_frame_anglerate</setFunction>
+ <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_DCM_ANGLE</typeName>
+ <sendFunction>mavlink_send_dcm_angle</sendFunction>
+ <setFunction>mavlink_set_dcm_angle</setFunction>
+ <mavlinkType>MAVLINK_TYPE_INT32_T</mavlinkType>
+ </udbType>
+ <udbType>
+ <typeName>UDB_TYPE_FLOAT</typeName>
+ <sendFunction>mavlink_send_param_float</sendFunction>
+ <setFunction>mavlink_set_param_float</setFunction>
+ <mavlinkType>MAVLINK_TYPE_FLOAT</mavlinkType>
+ </udbType>
+</udbTypes>
+
+<parameterBlocks>
+ <parameterBlock>
+ <blockName>PID_SETTINGS</blockName>
+ <storage_area>CONTROL_GAINS</storage_area>
+ <serialisationFlags>
+ <serialisationFlag>LOAD_AT_STARTUP</serialisationFlag>
+ <serialisationFlag>LOAD_AT_REBOOT</serialisationFlag>
+ <serialisationFlag>STORE_CALIB</serialisationFlag>
+ </serialisationFlags>
+ <load_callback>NULL</load_callback>
+ <in_mavlink_parameters>true</in_mavlink_parameters>
+ <parameters>
+ <parameter>
+ <parameterName>PID_ROLLKP</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>rollkp</variable_name>
+ <description>Roll proporational gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_ROLLKD</parameterName>
+ <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
+ <variable_name>rollkd</variable_name>
+ <description>Roll differential gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_YAWKPAIL</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>yawkpail</variable_name>
+ <description>yaw to aileron gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_YAWKDAIL</parameterName>
+ <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
+ <variable_name>yawkdail</variable_name>
+ <description>yaw to aileron rate gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_PITCHGAIN</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>pitchgain</variable_name>
+ <description>PItch gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_PITCHKD</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>pitchkd</variable_name>
+ <description>Pitch rate gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_ROLLKPRUD</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>rollkprud</variable_name>
+ <description>Roll differential gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_YAWKPRUD</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>yawkprud</variable_name>
+ <description>Yaw to rudder gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_YAWKDRUD</parameterName>
+ <udb_param_type>UDB_TYPE_GYROSCALE_Q14</udb_param_type>
+ <variable_name>yawkdrud</variable_name>
+ <description>Yaw to rudder rate gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
+ </parameter>
+ <parameter>
+ <parameterName>PID_ROLLKPRUD</parameterName>
+ <udb_param_type>UDB_TYPE_Q14</udb_param_type>
+ <variable_name>rollkprud</variable_name>
+ <description>Roll to rudder gain</description>
+ <min>0.0</min>
+ <max>0.5</max>
+ <readonly>false</readonly>
***The diff for this file has been truncated for email.***
=======================================
--- /trunk/libDCM/estAltitude.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/estAltitude.c Sat Aug 1 04:50:37 2015 UTC
@@ -44,6 +44,9 @@
inline long get_barometer_altitude(void) { return barometer_altitude; }
inline long get_barometer_agl_altitude(void) { return
barometer_agl_altitude; }

+/**
+ * @brief Ascertain a reference ambient barometric pressure & temperature
+ */
void altimeter_calibrate(void)
{
int ground_altitude = alt_origin.WW / 100; // meters
@@ -63,6 +66,11 @@
}
#endif

+/**
+ * @brief Calculate an altitude estimate using barometer data
+ * @param
+ * @return
+ */
void estAltitude(void)
{
#if (BAROMETER_ALTITUDE == 1)
=======================================
--- /trunk/libDCM/estWind.c Fri Oct 3 05:14:06 2014 UTC
+++ /trunk/libDCM/estWind.c Sat Aug 1 04:50:37 2015 UTC
@@ -35,7 +35,7 @@

#define MINROTATION ((int16_t)(0.2 * RMAX))

-void estWind(void)
+void estWind(int16_t angleOfAttack)
{
int16_t index;
int16_t groundVelocity[3];
@@ -65,6 +65,14 @@
fuselageDirection[1] = rmat[4];
fuselageDirection[2] = -rmat[7];

+ // adjust "fuselage direction" for angle of attack
+ longaccum.WW = (__builtin_mulss(- rmat[2], angleOfAttack)) << 2;
+ fuselageDirection[0] += longaccum._.W1;
+ longaccum.WW = (__builtin_mulss( rmat[5], angleOfAttack)) << 2;
+ fuselageDirection[1] += longaccum._.W1;
+ longaccum.WW = (__builtin_mulss(- rmat[8], angleOfAttack)) << 2;
+ fuselageDirection[2] += longaccum._.W1;
+
for (index = 0; index < 3; index++)
{
groundVelocity[index] >>= 1;
=======================================
--- /trunk/libDCM/estWind.h Mon Dec 15 11:31:28 2014 UTC
+++ /trunk/libDCM/estWind.h Sat Aug 1 04:50:37 2015 UTC
@@ -23,5 +23,5 @@

extern int16_t estimatedWind[3]; // wind velocity vectors in cm / sec

-void estWind(void);
+void estWind(int16_t angleOfAttack);

=======================================
--- /trunk/libDCM/gpsParseCommon.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/gpsParseCommon.c Sat Aug 1 04:50:37 2015 UTC
@@ -147,7 +147,7 @@
dcm_callback_gps_location_updated();

estLocation();
- estWind();
+ estWind(GetAofA());
estAltitude();
estYawDrift();

=======================================
--- /trunk/libDCM/gpsParseCommon.h Mon Dec 15 11:31:28 2014 UTC
+++ /trunk/libDCM/gpsParseCommon.h Sat Aug 1 04:50:37 2015 UTC
@@ -72,7 +72,7 @@
void gps_parse_common(void);
void gps_update_basic_data(void);
boolean gps_nav_capable_check_set(void);
-void HILSIM_set_gplane(void);
+void HILSIM_set_gplane(fractional gplane[]);
void HILSIM_set_omegagyro(void);

int32_t get_gps_date(void);
=======================================
--- /trunk/libDCM/gpsParseUBX.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/gpsParseUBX.c Sat Aug 1 04:50:37 2015 UTC
@@ -896,6 +896,28 @@
q_sim = q_sim_;
r_sim = r_sim_;
}
+
+void HILSIM_saturate(int16_t size, int16_t vector[3])
+{
+ // hardware 16 bit signed integer gyro and accelerometer data and offsets
+ // are divided by 2 prior to subtracting offsets from values.
+ // This is done to prevent overflow.
+ // However, it limits range to approximately +- RMAX.
+ // Data coming in from Xplane is in 16 bit signed integer format with
range +-2*RMAX,
+ // so it needs to be passed through a saturation computation that limits
to +-RMAX.
+ uint16_t index;
+ for (index = 0; index < size; index ++)
+ {
+ if (vector[index ] > RMAX)
+ {
+ vector[index] = RMAX;
+ }
+ if (vector[index] < -RMAX)
+ {
+ vector[index] = -RMAX;
+ }
+ }
+}

void hil_rc_input_adjust(char *inChannelName, int inChannelIndex, int
delta)
{
@@ -978,11 +1000,12 @@
}
}

-void HILSIM_set_gplane(void)
+void HILSIM_set_gplane(fractional gplane[])
{
gplane[0] = g_a_x_sim.BB;
gplane[1] = g_a_y_sim.BB;
gplane[2] = g_a_z_sim.BB;
+ HILSIM_saturate(3, gplane);
}

void HILSIM_set_omegagyro(void)
@@ -990,6 +1013,7 @@
omegagyro[0] = q_sim.BB;
omegagyro[1] = p_sim.BB;
omegagyro[2] = r_sim.BB;
+ HILSIM_saturate(3, omegagyro);
}
#endif // HILSIM

=======================================
--- /trunk/libDCM/libDCM.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/libDCM.c Sat Aug 1 04:50:37 2015 UTC
@@ -22,17 +22,18 @@
#include "libDCM.h"
#include "gpsData.h"
#include "gpsParseCommon.h"
+#include "../libUDB/heartbeat.h"
+#include "../libUDB/magnetometer.h"
+#include "../libUDB/barometer.h"
+#include "../libUDB/ADchannel.h"
#include "estAltitude.h"
#include "mathlibNAV.h"
#include "rmat.h"
#include "mag_drift.h"
-#include "../libUDB/barometer.h"
-#include "../libUDB/ADchannel.h"
-#include "../libUDB/heartbeat.h"
-#include "../libUDB/magnetometer.h"


union dcm_fbts_word dcm_flags;
+int16_t angleOfAttack;

// Calibrate for 10 seconds before moving servos
#define CALIB_COUNT 400 // 10 seconds at 40 Hz
@@ -40,6 +41,15 @@

void send_HILSIM_outputs(void);

+void SetAofA(int16_t AofA)
+{
+ angleOfAttack = AofA;
+}
+
+int16_t GetAofA(void)
+{
+ return angleOfAttack;
+}

void dcm_init(void)
{
@@ -133,7 +143,7 @@
// when we move the IMU step to the MPU call back, to run at 200 Hz,
remove this
if (dcm_flags._.calib_finished)
{
- dcm_run_imu_step();
+ dcm_run_imu_step(angleOfAttack);
}

dcm_heartbeat_callback(); // this was called
dcm_servo_callback_prepare_outputs();
=======================================
--- /trunk/libDCM/libDCM.h Fri Oct 3 05:14:06 2014 UTC
+++ /trunk/libDCM/libDCM.h Sat Aug 1 04:50:37 2015 UTC
@@ -36,6 +36,9 @@
// specifics of the MatrixPilot application.


+void SetAofA(int16_t AofA);
+int16_t GetAofA(void);
+

////////////////////////////////////////////////////////////////////////////////
// Functions
void dcm_init(void);
=======================================
--- /trunk/libDCM/mag_drift.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/mag_drift.h Sat Aug 1 04:50:37 2015 UTC
@@ -1,5 +1,26 @@
+// This file is part of MatrixPilot.
+//
+// http://code.google.com/p/gentlenav/
+//
+// Copyright 2009-2015 MatrixPilot Team
+// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
+//
+// MatrixPilot is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// MatrixPilot is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with MatrixPilot. If not, see <http://www.gnu.org/licenses/>.


+#ifndef MAG_DRIFT_H
+#define MAG_DRIFT_H


extern union intbb dcm_declination_angle;
@@ -15,3 +36,6 @@
void mag_drift_init(void);
void mag_drift(fractional errorYawplane[]);
void mag_drift_callback(void);
+
+
+#endif // MAG_DRIFT_H
=======================================
--- /trunk/libDCM/rmat.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/rmat.c Sat Aug 1 04:50:37 2015 UTC
@@ -116,9 +116,11 @@

// gravity, as measured in plane coordinate system
#ifdef INITIALIZE_VERTICAL // VTOL vertical initialization
-fractional gplane[] = { 0, -GRAVITY, 0 };
+static fractional gplane[] = { 0, -GRAVITY, 0 };
+int16_t aero_force[] = { 0 , GRAVITY , 0 };
#else // horizontal initialization
-fractional gplane[] = { 0, 0, GRAVITY };
+static fractional gplane[] = { 0, 0, GRAVITY };
+int16_t aero_force[] = { 0 , 0 , -GRAVITY };
#endif

// horizontal velocity over ground, as measured by GPS (Vz = 0)
@@ -189,7 +191,7 @@
static inline void read_accel(void)
{
#if (HILSIM == 1)
- HILSIM_set_gplane();
+ HILSIM_set_gplane(gplane);
// gplane[0] = g_a_x_sim.BB;
// gplane[1] = g_a_y_sim.BB;
// gplane[2] = g_a_z_sim.BB;
@@ -198,6 +200,9 @@
gplane[1] = YACCEL_VALUE;
gplane[2] = ZACCEL_VALUE;
#endif
+ aero_force[0] = - gplane[0];
+ aero_force[1] = - gplane[1];
+ aero_force[2] = - gplane[2];

#ifdef CATAPULT_LAUNCH_ENABLE
if (gplane[1] < -(GRAVITY/2))
@@ -241,13 +246,13 @@
read_accel();
}

-static int16_t omegaSOG(int16_t omega, uint16_t speed)
+static int16_t omegaSOG(int16_t omega, int16_t speed)
{
// multiplies omega times speed, and scales appropriately
// omega in radians per second, speed in cm per second
union longww working;
speed = speed >> 3;
- working.WW = __builtin_mulsu(omega, speed);
+ working.WW = __builtin_mulss(omega, speed);
if (((int16_t)working._.W1) > ((int16_t)CENTRIFSAT))
{
return RMAX;
@@ -265,13 +270,100 @@
}
}

-static void adj_accel(void)
+#if (CENTRIFUGAL_WITHOUT_GPS == 1)
+static void adj_accel(int16_t angleOfAttack)
{
+ // Performs centrifugal compensation without a GPS.
+ // Based on the fact that the magnitude of the
+ // compensated gplane vector should be GRAVITY*GRAVITY.
+ // This produces another equation from which the
+ // product of airspeed time rotation rate can be reasonably estimated.
+ int16_t omega_times_velocity ; // it should be positive, but noise
+ // in the computations could produce neg
+ uint16_t radical;
+ union longww accum;
+ int16_t accelY;
+ int16_t vertical_cross_rotation_axis;
+ int16_t force_cross_rotation_axis;
+ int16_t rotation_axis[2];
+
+ // Compute the X-Z rotation axis
+ // by normalizing the X-Z gyro vector
+ rotation_axis[0] = omegagyro[0];
+ rotation_axis[1] = omegagyro[2];
+ vector2_normalize(rotation_axis, rotation_axis);
+
+ // compute force cross rotation axis:
+ accum.WW = (__builtin_mulss(gplane[0], rotation_axis[1]) -
__builtin_mulss( gplane[2] , rotation_axis[0] ) ) << 2;
+ force_cross_rotation_axis = accum._.W1;
+
+ // compute vertical cross rotation axis:
+ accum.WW = (__builtin_mulss(rmat[6], rotation_axis[1]) -
__builtin_mulss(rmat[8], rotation_axis[0] ) ) << 2;
+ vertical_cross_rotation_axis = accum._.W1;
+
+ // compute the square root of the sum of the square of the
+ // force cross rotation, minus the square of the magnitude of the
accelerometer vector,
+ // plus the square of GRAVITY
+
+ // Start by using rmat for accelY instead of the measured value.
+ // It is less sensitive to forward acceleration, which cannot be
compensated without GPS.
+ accum.WW = (__builtin_mulsu( rmat[7], GRAVITY ) ) << 2;
+ accelY = accum._.W1;
+
+ // form the sum
+ accum.WW = __builtin_mulss(force_cross_rotation_axis,
force_cross_rotation_axis)
+ + __builtin_muluu(GRAVITY, GRAVITY)
+ - __builtin_mulss(gplane[0], gplane[0])
+ - __builtin_mulss(gplane[2], gplane[2])
+ - __builtin_mulss(accelY, accelY);
+ if (accum.WW < 0)
+ {
+ accum.WW = 0;
+ }
+ radical = sqrt_long((uint32_t)accum.WW);
+
+ // Now we are using the solution to quadratic equation in the theory,
+ // and there is some logic for selecting the positive or negative square
root
+ if (force_cross_rotation_axis < 0)
+ {
+ omega_times_velocity = force_cross_rotation_axis + radical;
+ }
+ else
+ {
+ if (vertical_cross_rotation_axis < 0)
+ {
+ omega_times_velocity = force_cross_rotation_axis + radical;
+ }
+ else
+ {
+ omega_times_velocity = force_cross_rotation_axis - radical;
+ }
+ }
+ if (omega_times_velocity < 0)
+ {
+ omega_times_velocity = 0;
+ }
+ // now compute omega vector cross velocity vector and adjust
+ accum.WW = (__builtin_mulss(omega_times_velocity , rotation_axis[1] ) )
<< 2;
+ gplane[0] = gplane[0] - accum._.W1;
+ accum.WW = (__builtin_mulss(omega_times_velocity , rotation_axis[0] ) )
<< 2;
+ gplane[0] = gplane[0] + accum._.W1;
+}
+#else
+static void adj_accel(int16_t angleOfAttack)
+{
+ union longww accum;
+ int16_t air_speed_z;
// total (3D) airspeed in cm/sec is used to adjust for acceleration
- gplane[0] = gplane[0] - omegaSOG(omegaAccum[2], air_speed_3DGPS);
+ // compute Z component of airspeed due to angle of attack
+ accum.WW = __builtin_mulsu(angleOfAttack, air_speed_3DGPS) << 2;
+ air_speed_z = accum._.W1;
+ // compute centrifugal and forward acceleration compensation
+ gplane[0] = gplane[0] - omegaSOG(omegaAccum[2], air_speed_3DGPS)+
omegaSOG(omegaAccum[1], air_speed_z);
gplane[2] = gplane[2] + omegaSOG(omegaAccum[0], air_speed_3DGPS);
- gplane[1] = gplane[1] + ((uint16_t)(ACCELSCALE)) * forward_acceleration;
+ gplane[1] = gplane[1] - omegaSOG(omegaAccum[0], air_speed_z) +
((uint16_t)(ACCELSCALE)) * forward_acceleration;
}
+#endif // CENTRIFUGAL_WITHOUT_GPS

// The update algorithm!!
static void rupdate(void)
@@ -520,12 +612,12 @@

extern void mag_drift(fractional errorYawplane[]);

-void dcm_run_imu_step(void)
+void dcm_run_imu_step(int16_t angleOfAttack)
{
// update the matrix, renormalize it, adjust for roll and
// pitch drift, and send it to the servos.
dead_reckon(); // in libDCM:deadReconing.c
- adj_accel(); // local
+ adj_accel(angleOfAttack); // local
rupdate(); // local
normalize(); // local
roll_pitch_drift(); // local
=======================================
--- /trunk/libDCM/rmat.h Wed May 6 15:02:33 2015 UTC
+++ /trunk/libDCM/rmat.h Sat Aug 1 04:50:37 2015 UTC
@@ -40,19 +40,21 @@

#if (MAG_YAW_DRIFT == 1)
extern fractional magFieldEarth[3];
+void udb_magnetometer_callback(void);
#endif

extern fractional rmat[]; // gyro rotation vector:
extern fractional omegaAccum[]; // accumulator for computing
adjusted omega:
extern fractional omegagyro[];
extern fractional accelEarth[]; // acceleration, as measured
in GPS earth coordinate system
-extern fractional gplane[];
+//extern fractional gplane[];
+extern int16_t aero_force[];
extern fractional dirOverGndHGPS[]; // horizontal velocity over
ground, as measured by GPS (Vz = 0 )
extern fractional dirOverGndHrmat[]; // horizontal direction over
ground, as indicated by Rmatrix
extern union intbb dcm_declination_angle; // Declination +-32767 =
+-360deg

void dcm_init_rmat(void);
-void dcm_run_imu_step(void);
+void dcm_run_imu_step(int16_t angleOfAttack);
void yaw_drift_reset(void);

// Calibrate the sensors
=======================================
--- /trunk/libSTM/libSTM.c Wed May 6 15:02:33 2015 UTC
+++ /trunk/libSTM/libSTM.c Sat Aug 1 04:50:37 2015 UTC
@@ -414,5 +414,13 @@
}

//void _init(void) {}
+
+void udb_init_USART(void)
+{
+}
+
+void udb_init_GPS(void)
+{
+}

#endif // (BOARD_TYPE == PX4_BOARD)
=======================================
***Additional files exist in this changeset.***