[gentlenav] r3652 committed - MatrixPilot_helicalTurns: updated branch with latest work in WJP maste...

[gent...@googlecode.com]

Revision: 3652
Author: robert.d...@gmail.com
Date: Sun Jun 14 13:43:17 2015 UTC
Log: MatrixPilot_helicalTurns: updated branch with latest work in WJP
master version.
https://code.google.com/p/gentlenav/source/detail?r=3652

Added:
/branches/MatrixPilot_helicalTurns/RollPitchYaw/module.mk
/branches/MatrixPilot_helicalTurns/Tools/Build/proj_gen.bat
/branches/MatrixPilot_helicalTurns/Tools/Build/proj_gen.sh
/branches/MatrixPilot_helicalTurns/Tools/Build/template.sln
/branches/MatrixPilot_helicalTurns/Tools/LedTest/module.mk
Deleted:
/branches/MatrixPilot_helicalTurns/Tools/Build/make-build.bat
/branches/MatrixPilot_helicalTurns/Tools/Build/make-build.sh
Modified:
/branches/MatrixPilot_helicalTurns/Config/Cessna/mavlink_options.h
/branches/MatrixPilot_helicalTurns/MatrixPilot/config_tests.c
/branches/MatrixPilot_helicalTurns/MatrixPilot/helicalTurnCntrl.c
/branches/MatrixPilot_helicalTurns/MatrixPilot/main.c
/branches/MatrixPilot_helicalTurns/MatrixPilot/minIni.c
/branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.c
/branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.h

/branches/MatrixPilot_helicalTurns/Microchip/USB/CDC-Device-Driver/usb_function_cdc.c

/branches/MatrixPilot_helicalTurns/Microchip/USB/MSD-Device-Driver/usb_function_msd.c
/branches/MatrixPilot_helicalTurns/Tools/Build/mplab8-template.txt
/branches/MatrixPilot_helicalTurns/Tools/Build/pyProjectCreator.py
/branches/MatrixPilot_helicalTurns/Tools/Build/template.ebp
/branches/MatrixPilot_helicalTurns/Tools/Build/template.vcxproj
/branches/MatrixPilot_helicalTurns/Tools/LedTest/Config/options.h
/branches/MatrixPilot_helicalTurns/Tools/LedTest/main.c
/branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-I2C1.c
/branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-serial.c
/branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.c
/branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.h
/branches/MatrixPilot_helicalTurns/device-AUAV3.mk
/branches/MatrixPilot_helicalTurns/libDCM/estAirspeed.c
/branches/MatrixPilot_helicalTurns/libUDB/ConfigPX4.h
/branches/MatrixPilot_helicalTurns/libUDB/background.c

=======================================
--- /dev/null
+++ /branches/MatrixPilot_helicalTurns/RollPitchYaw/module.mk Sun Jun 14
13:43:17 2015 UTC
@@ -0,0 +1,12 @@
+# this file is included from makefile
+
+local_src := $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.c)
+
+ifneq (,$(filter $(TOOLCHAIN), C30 XC16))
+local_src += $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.s)
+else
+endif
+
+local_src += $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.sm)
+
+$(eval $(call make-target,$(subdirectory)/$(subdirectory).a,$(local_src)))
=======================================
--- /dev/null
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/proj_gen.bat Sun Jun 14
13:43:17 2015 UTC
@@ -0,0 +1,24 @@
+@echo off
+
+pyProjectCreator.py -r ../.. -t SIL -c Config/Cessna -d USE_LOGO
+pyProjectCreator.py -r ../.. -t PX4 -c Config/Cessna
+pyProjectCreator.py -r ../.. -t UDB4 -c Config/Cessna -d NOFS=1
+
+@rem goto END
+
+pyProjectCreator.py -r ../.. -t UDB5 -c Config/Cessna -d NOFS=1
+pyProjectCreator.py -r ../.. -t UDB5 -c Config/CloudsFly -d NOFS=1
+
+pyProjectCreator.py -r ../.. -t AUAV3 -c Config/Cessna -d USE_NORADIO
+pyProjectCreator.py -r ../.. -t AUAV3
+
+pyProjectCreator.py -r ../.. -n RollPitchYaw -t UDB4
+pyProjectCreator.py -r ../.. -n RollPitchYaw -t UDB5
+pyProjectCreator.py -r ../.. -n RollPitchYaw -t AUAV3
+
+pyProjectCreator.py -r ../.. -n LedTest -t UDB4
+pyProjectCreator.py -r ../.. -n LedTest -t UDB5
+pyProjectCreator.py -r ../.. -n LedTest -t AUAV3
+
+:END
+pause
=======================================
--- /dev/null
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/proj_gen.sh Sun Jun 14
13:43:17 2015 UTC
@@ -0,0 +1,15 @@
+#!/bin/bash
+
+python pyProjectCreator.py -t SIL -r ../..
+
+python pyProjectCreator.py -t UDB4 -r ../..
+python pyProjectCreator.py -t UDB5 -r ../..
+python pyProjectCreator.py -t AUAV3 -r ../..
+
+python pyProjectCreator.py -n RollPitchYaw -t UDB4 -r ../..
+python pyProjectCreator.py -n RollPitchYaw -t UDB5 -r ../..
+python pyProjectCreator.py -n RollPitchYaw -t AUAV3 -r ../..
+
+python pyProjectCreator.py -n LedTest -t UDB4 -r ../..
+python pyProjectCreator.py -n LedTest -t UDB5 -r ../..
+python pyProjectCreator.py -n LedTest -t AUAV3 -r ../..
=======================================
--- /dev/null
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/template.sln Sun Jun 14
13:43:17 2015 UTC
@@ -0,0 +1,20 @@
+
+Microsoft Visual Studio Solution File, Format Version 11.00
+# Visual C++ Express 2010
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}")
= "%%PROJECT%%", "%%PROJECT%%.vcxproj", "{81C72BCE-FAF2-423A-B311-50BA1F7A010D}"
+EndProject
+Global
+ GlobalSection(SolutionConfigurationPlatforms) = preSolution
+ Debug|Win32 = Debug|Win32
+ Release|Win32 = Release|Win32
+ EndGlobalSection
+ GlobalSection(ProjectConfigurationPlatforms) = postSolution
+ {81C72BCE-FAF2-423A-B311-50BA1F7A010D}.Debug|Win32.ActiveCfg = Debug|
Win32
+ {81C72BCE-FAF2-423A-B311-50BA1F7A010D}.Debug|Win32.Build.0 = Debug|Win32
+ {81C72BCE-FAF2-423A-B311-50BA1F7A010D}.Release|Win32.ActiveCfg = Release|
Win32
+ {81C72BCE-FAF2-423A-B311-50BA1F7A010D}.Release|Win32.Build.0 = Release|
Win32
+ EndGlobalSection
+ GlobalSection(SolutionProperties) = preSolution
+ HideSolutionNode = FALSE
+ EndGlobalSection
+EndGlobal
=======================================
--- /dev/null
+++ /branches/MatrixPilot_helicalTurns/Tools/LedTest/module.mk Sun Jun 14
13:43:17 2015 UTC
@@ -0,0 +1,12 @@
+# this file is included from makefile
+
+local_src := $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.c)
+
+ifneq (,$(filter $(TOOLCHAIN), C30 XC16))
+local_src += $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.s)
+else
+endif
+
+local_src += $(wildcard $(SOURCE_DIR)/$(subdirectory)/*.sm)
+
+$(eval $(call make-target,$(subdirectory)/$(subdirectory).a,$(local_src)))
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/make-build.bat Fri May
1 02:12:19 2015 UTC
+++ /dev/null
@@ -1,24 +0,0 @@
-@echo off
-
-pyProjectCreator.py -r ../.. -t SIL -c Config/Cessna -d USE_LOGO
-pyProjectCreator.py -r ../.. -t PX4 -c Config/Cessna
-pyProjectCreator.py -r ../.. -t UDB4 -c Config/Cessna -d NOFS=1
-
-@rem goto END
-
-pyProjectCreator.py -r ../.. -t UDB5 -c Config/Cessna -d NOFS=1
-pyProjectCreator.py -r ../.. -t UDB5 -c Config/CloudsFly -d NOFS=1
-
-pyProjectCreator.py -r ../.. -t AUAV3 -c Config/Cessna -d USE_NORADIO
-pyProjectCreator.py -r ../.. -t AUAV3
-
-pyProjectCreator.py -r ../.. -n RollPitchYaw -t UDB4
-pyProjectCreator.py -r ../.. -n RollPitchYaw -t UDB5
-pyProjectCreator.py -r ../.. -n RollPitchYaw -t AUAV3
-
-pyProjectCreator.py -r ../.. -n LedTest -t UDB4
-pyProjectCreator.py -r ../.. -n LedTest -t UDB5
-pyProjectCreator.py -r ../.. -n LedTest -t AUAV3
-
-:END
-pause
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/make-build.sh Sat Mar 14
12:37:08 2015 UTC
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/bin/bash
-
-python pyProjectCreator.py -t SIL -r ../..
-
-python pyProjectCreator.py -t UDB4 -r ../..
-python pyProjectCreator.py -t UDB5 -r ../..
-python pyProjectCreator.py -t AUAV3 -r ../..
-
-python pyProjectCreator.py -n RollPitchYaw -t UDB4 -r ../..
-python pyProjectCreator.py -n RollPitchYaw -t UDB5 -r ../..
-python pyProjectCreator.py -n RollPitchYaw -t AUAV3 -r ../..
-
-python pyProjectCreator.py -n LedTest -t UDB4 -r ../..
-python pyProjectCreator.py -n LedTest -t UDB5 -r ../..
-python pyProjectCreator.py -n LedTest -t AUAV3 -r ../..
=======================================
--- /branches/MatrixPilot_helicalTurns/Config/Cessna/mavlink_options.h Fri
May 1 02:12:19 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Config/Cessna/mavlink_options.h Sun
Jun 14 13:43:17 2015 UTC
@@ -23,7 +23,9 @@
// mavlink_options.h
//

+#ifndef USE_MAVLINK
#define USE_MAVLINK 1
+#endif

// 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
=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/config_tests.c Mon May
4 02:37:20 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/config_tests.c Sun Jun
14 13:43:17 2015 UTC
@@ -240,3 +240,6 @@
#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
=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/helicalTurnCntrl.c Wed
May 6 14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/helicalTurnCntrl.c Sun
Jun 14 13:43:17 2015 UTC
@@ -224,9 +224,10 @@

// 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 ];
@@ -241,15 +242,43 @@
{
steeringInput = 0;
}
- }
- 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) >> 1;
+ }
+ 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) >> 1));
+#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)
@@ -334,7 +363,7 @@
pitchAdjustAngleOfAttack = 0;
elevatorLoadingTrim = 0;
}
- SetAofA(angleOfAttack);
+// SetAofA(angleOfAttack); // removed by helicalTurns

// convert desired turn rate from radians/second to gyro units

=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/main.c Wed May 6
14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/main.c Sun Jun 14
13:43:17 2015 UTC
@@ -34,6 +34,7 @@
#include "defines.h"
#include "behaviour.h"
#include "telemetry.h"
+#include "servoMix.h"
#include "servoPrepare.h"
#include "../libDCM/gpsParseCommon.h"
#include "../libUDB/serialIO.h"
=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/minIni.c Wed May 6
14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/minIni.c Sun Jun 14
13:43:17 2015 UTC
@@ -353,8 +353,6 @@
* \param Key the name of the entry to find the value of
* \param DefValue default value in the event of a failed read; it
should
* zero (0) or one (1).
- * \param Buffer a pointer to the buffer to copy into
- * \param BufferSize the maximum number of characters to copy
* \param Filename the name and full path of the .ini file to read from
*
* A true boolean is found if one of the following is matched:
=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.c Mon May 4
02:37:20 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.c Sun Jun 14
13:43:17 2015 UTC
@@ -33,8 +33,6 @@
//
// Mix computed roll and pitch controls into the output channels for the
compiled airframe type.

-//const int16_t elevatorbgain = (int16_t)(8.0*gains.ElevatorBoost);
-//const int16_t rudderbgain = (int16_t)(8.0*gains.RudderBoost);
static int16_t elevatorbgain = 0;
static int16_t rudderbgain = 0;

@@ -57,106 +55,160 @@
else
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] = udb_pwTrim[AILERON_INPUT_CHANNEL] ; //
in fly by wire or navigate mode, manual input is calculated as part of 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;
- }

// 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)
- 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);
+ // 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);

- temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ 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[RUDDER_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
- udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
+ udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
- {
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
- }
- else
- {
- temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
- }
-#endif
+ temp = pwManual[RUDDER_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
+ udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+
+ if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
+ {
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
+ }
+ else
+ {
+ temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ }
+#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)
{
- int32_t vtail_yaw_control;
+ int16_t rudderInput;
+ int16_t elevatorInput;
+ int16_t pitchInput;
+ int16_t yawInput;
+ int16_t pitchCommand;
+ int16_t yawCommand;
+ int32_t vtail_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);

- 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);
+ // 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[ELEVATOR_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control +
vtail_yaw_control);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ // 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[RUDDER_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitch_control -
vtail_yaw_control);
- udb_pwOut[RUDDER_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 = 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)
- {
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
- }
- else
- {
- temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
- }
+ if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
+ {
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
+ }
+ else
+ {
+ 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_VTAIL

// Delta-Wing airplane airframe
// 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);
- udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ 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);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ 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);
- udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
-
- if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
- {
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
- }
- else
- {
- temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
- }
-#endif
+ temp = pwManual[RUDDER_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
+ udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+
+ if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
+ {
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
+ }
+ else
+ {
+ 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
// Mix half of roll_control and half of pitch_control into aileron
channels
@@ -187,7 +239,7 @@
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_HELI

udb_pwOut[PASSTHROUGH_A_OUTPUT_CHANNEL] =
udb_servo_pulsesat(pwManual[PASSTHROUGH_A_INPUT_CHANNEL]);
udb_pwOut[PASSTHROUGH_B_OUTPUT_CHANNEL] =
udb_servo_pulsesat(pwManual[PASSTHROUGH_B_INPUT_CHANNEL]);
=======================================
--- /branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.h Sat Apr 18
15:53:51 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/MatrixPilot/servoMix.h Sun Jun 14
13:43:17 2015 UTC
@@ -24,6 +24,7 @@


void servoMix(void);
+void servoMix_init(void);
void cameraServoMix(void);


=======================================
---
/branches/MatrixPilot_helicalTurns/Microchip/USB/CDC-Device-Driver/usb_function_cdc.c
Sun Mar 15 05:35:25 2015 UTC
+++
/branches/MatrixPilot_helicalTurns/Microchip/USB/CDC-Device-Driver/usb_function_cdc.c
Sun Jun 14 13:43:17 2015 UTC
@@ -101,7 +101,7 @@

********************************************************************/

-#if defined(__dsPIC33E__)
+#if defined(__dsPIC33E__)

/** I N C L U D E S
**********************************************************/
#include "USB/usb.h"
@@ -1054,6 +1054,6 @@

#endif //USB_USE_CDC

-#endif // defined(__dsPIC33E__)
+#endif // defined(__dsPIC33E__)

/** EOF cdc.c
****************************************************************/
=======================================
---
/branches/MatrixPilot_helicalTurns/Microchip/USB/MSD-Device-Driver/usb_function_msd.c
Wed Mar 18 13:16:55 2015 UTC
+++
/branches/MatrixPilot_helicalTurns/Microchip/USB/MSD-Device-Driver/usb_function_msd.c
Sun Jun 14 13:43:17 2015 UTC
@@ -93,7 +93,7 @@

********************************************************************/

-#if defined(__dsPIC33E__)
+#if defined(__dsPIC33E__)

/** I N C L U D E S **************************************************/
#include "USB/usb.h"
@@ -1234,22 +1234,22 @@

/******************************************************************************
Function:
BYTE MSDWriteHandler(void)
-
+
Description:
This funtion processes a write command received through
the MSD class driver
-
+
PreCondition:
None
-
+
Parameters:
None
-
+
Return Values:
BYTE - the current state of the MSDWriteHandler state
machine. The valid values are defined in MSD.h under the
MSDWriteHandler state machine declaration section
-
+
Remarks:
None

@@ -1330,20 +1330,20 @@
//We finished receiving a sector worth of data from the host.
//Check if the media is write protected before deciding what
//to do with the data.
- if(LUNWriteProtectState())
+ if(LUNWriteProtectState())
{
//The device appears to be write protected.
//Let host know error occurred. The bCSWStatus flag is
also used by
//the write handler, to know not to even attempt the
write sequence.
- msd_csw.bCSWStatus=0x01;
-
+ msd_csw.bCSWStatus=0x01;
+
//Set sense keys so the host knows what caused the
error.
gblSenseData[LUN_INDEX].SenseKey=S_NOT_READY;
gblSenseData[LUN_INDEX].ASC=ASC_WRITE_PROTECTED;
gblSenseData[LUN_INDEX].ASCQ=ASCQ_WRITE_PROTECTED;
}
- MSDWriteState = MSD_WRITE10_SECTOR;
- break;
+ MSDWriteState = MSD_WRITE10_SECTOR;
+ break;
}
}
//Fall through to MSD_WRITE10_RX_PACKET
@@ -1352,8 +1352,8 @@
{
break;
}
-
-
gblCBW.dCBWDataTransferLength-=USBHandleGetLength(USBMSDOutHandle); // 64B
read
+
+
gblCBW.dCBWDataTransferLength-=USBHandleGetLength(USBMSDOutHandle); // 64B
read
msd_csw.dCSWDataResidue-=USBHandleGetLength(USBMSDOutHandle);
ptrNextData += MSD_OUT_EP_SIZE;

@@ -1388,56 +1388,55 @@
gblSenseData[LUN_INDEX].SenseKey=S_MEDIUM_ERROR;

gblSenseData[LUN_INDEX].ASC=ASC_NO_ADDITIONAL_SENSE_INFORMATION;

gblSenseData[LUN_INDEX].ASCQ=ASCQ_NO_ADDITIONAL_SENSE_INFORMATION;
- }
+ }
}
}
-
+
//One LBA is written (unless an error occurred). Advance state
//variables so we can eventually finish handling the CBW
request.
- LBA.Val++;
- TransferLength.Val--;
+ LBA.Val++;
+ TransferLength.Val--;
MSDWriteState = MSD_WRITE10_BLOCK;
break;
- }
-
+ }
+
default:
//Illegal condition which should not occur. If for some
reason it
//does, try to let the host know know an error has occurred.
msd_csw.bCSWStatus=0x02; //Phase Error
USBStallEndpoint(MSD_DATA_OUT_EP, OUT_FROM_HOST);
- MSDWriteState = MSD_WRITE10_WAIT;
+ MSDWriteState = MSD_WRITE10_WAIT;
}
-
return MSDWriteState;
}




/******************************************************************************
- Function:
- void ResetSenseData(void)
-
- Description:
- This routine resets the Sense Data, initializing the
- structure RequestSenseResponse gblSenseData.
-
- PreCondition:
- None
-
- Parameters:
- None
-
- Return Values:
- None
-
- Remarks:
- None
-
+ Function:
+ void ResetSenseData(void)
+
+ Description:
+ This routine resets the Sense Data, initializing the
+ structure RequestSenseResponse gblSenseData.
+
+ PreCondition:
+ None
+
+ Parameters:
+ None
+
+ Return Values:
+ None
+
+ Remarks:
+ None
+

*****************************************************************************/
-void ResetSenseData(void)
+void ResetSenseData(void)
{
gblSenseData[LUN_INDEX].ResponseCode=S_CURRENT;
- gblSenseData[LUN_INDEX].VALID=0; // no data in the information field
+ gblSenseData[LUN_INDEX].VALID=0; // no data in the information field
gblSenseData[LUN_INDEX].Obsolete=0x0;
gblSenseData[LUN_INDEX].SenseKey=S_NO_SENSE;
//gblSenseData.Resv;
@@ -1448,7 +1447,7 @@
gblSenseData[LUN_INDEX].InformationB1=0x00;
gblSenseData[LUN_INDEX].InformationB2=0x00;
gblSenseData[LUN_INDEX].InformationB3=0x00;
- gblSenseData[LUN_INDEX].AddSenseLen=0x0a; // n-7 (n=17 (0..17))
+ gblSenseData[LUN_INDEX].AddSenseLen=0x0a; // n-7 (n=17 (0..17))
gblSenseData[LUN_INDEX].CmdSpecificInfo.Val=0x0;
gblSenseData[LUN_INDEX].ASC=0x0;
gblSenseData[LUN_INDEX].ASCQ=0x0;
@@ -1461,55 +1460,55 @@



/******************************************************************************
- Function:
- BYTE MSDCheckForErrorCases(DWORD DeviceBytes)
-
- Description:
- This function can be called to check for various error cases,
primarily
- the "Thirteen Cases" errors described in the MSD BOT v1.0 specs. If
an
- error is detected, the function internally calls the MSDErrorHandler()
- handler function, to take care of appropriately responding to the
host,
- based on the error condition.
- PreCondition:
- None
-
- Parameters:
- DWORD DeviceBytes - Input: This is the total number of bytes the MSD
- device firmware is expecting in the MSD transfer.
- Return Values:
- BYTE - Returns a byte containing the error code. The possible error
- cases that can be detected and reported are:
+ Function:
+ BYTE MSDCheckForErrorCases(DWORD DeviceBytes)
+
+ Description:
+ This function can be called to check for various error cases, primarily
+ the "Thirteen Cases" errors described in the MSD BOT v1.0 specs. If an
+ error is detected, the function internally calls the MSDErrorHandler()
+ handler function, to take care of appropriately responding to the host,
+ based on the error condition.
+ PreCondition:
+ None
+
+ Parameters:
+ DWORD DeviceBytes - Input: This is the total number of bytes the MSD
+ device firmware is expecting in the MSD transfer.
+ Return Values:
+ BYTE - Returns a byte containing the error code. The possible error
+ cases that can be detected and reported are:
MSD_ERROR_CASE_NO_ERROR - None of the "Thirteen cases" errors
were detected
- MSD_ERROR_CASE_2
- MSD_ERROR_CASE_3
- MSD_ERROR_CASE_4
- MSD_ERROR_CASE_5
- MSD_ERROR_CASE_7
- MSD_ERROR_CASE_8
- MSD_ERROR_CASE_9
- MSD_ERROR_CASE_11
- MSD_ERROR_CASE_10
- MSD_ERROR_CASE_13
-
- Remarks:
- None
-
+ MSD_ERROR_CASE_2
+ MSD_ERROR_CASE_3
+ MSD_ERROR_CASE_4
+ MSD_ERROR_CASE_5
+ MSD_ERROR_CASE_7
+ MSD_ERROR_CASE_8
+ MSD_ERROR_CASE_9
+ MSD_ERROR_CASE_11
+ MSD_ERROR_CASE_10
+ MSD_ERROR_CASE_13
+
+ Remarks:
+ None
+

*****************************************************************************/
BYTE MSDCheckForErrorCases(DWORD DeviceBytes)
{
BYTE MSDErrorCase;
BOOL HostMoreDataThanDevice;
BOOL DeviceNoData;
-
+
//Check if device is expecting no data (Dn)
if(DeviceBytes == 0)
{
DeviceNoData = TRUE;
- }
+ }
else
{
DeviceNoData = FALSE;
- }
+ }

//First check for the three good/non-error cases

@@ -1517,7 +1516,7 @@
if((MSDHostNoData == TRUE) && (DeviceNoData == TRUE))
{
return MSD_ERROR_CASE_NO_ERROR;
- }
+ }

//Check for good cases where the data sizes between host and device
match
if(gblCBW.dCBWDataTransferLength == DeviceBytes)
@@ -1529,18 +1528,18 @@
if(MSDCommandState != MSD_WRITE_10)
{
return MSD_ERROR_CASE_NO_ERROR;
- }
+ }
}
- else //if(MSD_State == MSD_DATA_OUT)
+ else //if(MSD_State == MSD_DATA_OUT)
{
//Check for good case: Ho = Do (Case 12)
//Make sure Ho = Do, instead of Ho = Di
if(MSDCommandState == MSD_WRITE_10)
{
return MSD_ERROR_CASE_NO_ERROR;
- }
- }
- }
+ }
+ }
+ }

//If we get to here, this implies some kind of error is occuring. Do
some
//checks to find out which error occurred, so we know how to handle it.
@@ -1549,11 +1548,11 @@
if(gblCBW.dCBWDataTransferLength > DeviceBytes)
{
HostMoreDataThanDevice = TRUE;
- }
+ }
else
{
HostMoreDataThanDevice = FALSE;
- }
+ }

//Check host's expected data direction
if(MSD_State == MSD_DATA_OUT)
@@ -1561,7 +1560,7 @@
//First check for Ho <> Di (Case 10)
if((MSDCommandState != MSD_WRITE_10) && (DeviceNoData == FALSE))
MSDErrorCase = MSD_ERROR_CASE_10;
- //Check for Hn < Do (Case 3)
+ //Check for Hn < Do (Case 3)
else if(MSDHostNoData == TRUE)
MSDErrorCase = MSD_ERROR_CASE_3;
//Check for Ho > Dn (Case 9)
@@ -1578,7 +1577,7 @@
{
//First check for Hi <> Do (Case 8)
if(MSDCommandState == MSD_WRITE_10)
- MSDErrorCase = MSD_ERROR_CASE_8;
+ MSDErrorCase = MSD_ERROR_CASE_8;
//Check for Hn < Di (Case 2)
else if(MSDHostNoData == TRUE)
MSDErrorCase = MSD_ERROR_CASE_2;
@@ -1591,117 +1590,117 @@
//Check for Hi < Di (Case 7)
else //if(gblCBW.dCBWDataTransferLength < DeviceBytes)
MSDErrorCase = MSD_ERROR_CASE_7;
- }
+ }
//Now call the MSDErrorHandler(), based on the error that was detected.
MSDErrorHandler(MSDErrorCase);
return MSDErrorCase;
-}
+}



/******************************************************************************
- Function:
- void MSDErrorHandler(BYTE ErrorCase)
-
- Description:
- Once an error condition has been detected, this function can be
called
- to set the proper states and perform the proper tasks needed to let
the
- host know about the error.
- PreCondition:
- Firmware should have already determined an error occurred, and it should
- know what the error code was before calling this handler.
-
- Parameters:
- BYTE ErrorCase - Input: This is the error code that the firmware
- detected. This error code will determine how the
- handler will behave (ex: what status to send to
host,
- what endpoint(s) should be stalled, etc.).
- The implemented error case possibilities are (suffix
- numbers correspond to the "Thirteen cases" numbers
- described in the MSD BOT specs v1.0):
-
- MSD_ERROR_CASE_2
- MSD_ERROR_CASE_3
- MSD_ERROR_CASE_4
- MSD_ERROR_CASE_5
- MSD_ERROR_CASE_7
- MSD_ERROR_CASE_8
- MSD_ERROR_CASE_9
- MSD_ERROR_CASE_11
- MSD_ERROR_CASE_10
- MSD_ERROR_CASE_13
- MSD_ERROR_UNSUPPORTED_COMMAND
+ Function:
+ void MSDErrorHandler(BYTE ErrorCase)

- Return Values:
- None
-
- Remarks:
- None
-
+ Description:
+ Once an error condition has been detected, this function can be called
+ to set the proper states and perform the proper tasks needed to let
the
+ host know about the error.
+ PreCondition:
+ Firmware should have already determined an error occurred, and it
should
+ know what the error code was before calling this handler.
+
+ Parameters:
+ BYTE ErrorCase - Input: This is the error code that the firmware
+ detected. This error code will determine how the
+ handler will behave (ex: what status to send to
host,
+ what endpoint(s) should be stalled, etc.).
+ The implemented error case possibilities are
(suffix
+ numbers correspond to the "Thirteen cases" numbers
+ described in the MSD BOT specs v1.0):
+
+ MSD_ERROR_CASE_2
+ MSD_ERROR_CASE_3
+ MSD_ERROR_CASE_4
+ MSD_ERROR_CASE_5
+ MSD_ERROR_CASE_7
+ MSD_ERROR_CASE_8
+ MSD_ERROR_CASE_9
+ MSD_ERROR_CASE_1
+ MSD_ERROR_CASE_10
+ MSD_ERROR_CASE_13
+ MSD_ERROR_UNSUPPORTED_COMMAND
+
+ Return Values:
+ None
+
+ Remarks:
+ None
+

*****************************************************************************/
void MSDErrorHandler(BYTE ErrorCase)
{
- BYTE OldMSD_State;
-
+ BYTE OldMSD_State;
+
//Both MSD bulk IN and OUT endpoints should not be busy when these error
cases are detected
//If for some reason this isn't true, then we should preserve the state
machines states for now.
- if((USBHandleBusy(USBMSDInHandle)) || (USBHandleBusy(USBMSDOutHandle)))
- {
- return;
- }
+ if((USBHandleBusy(USBMSDInHandle)) || (USBHandleBusy(USBMSDOutHandle)))
+ {
+ return;
+ }

- //Save the old state before we change it. The old state is needed to
determine
- //the proper handling behavior in the case of receiving unsupported
commands.
- OldMSD_State = MSD_State;
+ //Save the old state before we change it. The old state is needed to
determine
+ //the proper handling behavior in the case of receiving unsupported
commands.
+ OldMSD_State = MSD_State;

//Reset main state machines back to idle values.
MSDCommandState = MSD_COMMAND_WAIT;
MSDReadState = MSD_READ10_WAIT;
MSDWriteState = MSD_WRITE10_WAIT;
//After the conventional 13 test cases failures, the host still expects a
valid CSW packet
- msd_csw.dCSWDataResidue = gblCBW.dCBWDataTransferLength; //Indicate
the unconsumed/unsent data
- msd_csw.bCSWStatus = MSD_CSW_COMMAND_FAILED; //Gets changed later
to phase error for errors that user phase error
- MSD_State = MSD_SEND_CSW;
+ msd_csw.dCSWDataResidue = gblCBW.dCBWDataTransferLength; //Indicate the
unconsumed/unsent data
+ msd_csw.bCSWStatus = MSD_CSW_COMMAND_FAILED; //Gets changed later to
phase error for errors that user phase error
+ MSD_State = MSD_SEND_CSW;

- //Now do other error related handling tasks, which depend on the
specific
- //error type that was detected.
+ //Now do other error related handling tasks, which depend on the specific
+ //error type that was detected.
switch(ErrorCase)
{
case MSD_ERROR_CASE_2://Also CASE_3
- msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
- break;
+ msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
+ break;
case MSD_ERROR_CASE_4://Also CASE_5
- USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST); //STALL the bulk IN
MSD endpoint
+ USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST); // STALL the bulk IN
MSD endpoint
break;
case MSD_ERROR_CASE_7://Also CASE_8
- msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
- USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST); //STALL the bulk IN
MSD endpoint
- break;
+ msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
+ USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST); // STALL the bulk IN
MSD endpoint
+ break;
case MSD_ERROR_CASE_9://Also CASE_11
USBStallEndpoint(MSD_DATA_OUT_EP, OUT_FROM_HOST); //Stall the bulk OUT
endpoint
break;
case MSD_ERROR_CASE_10://Also CASE_13
- msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
+ msd_csw.bCSWStatus = MSD_CSW_PHASE_ERROR;
USBStallEndpoint(MSD_DATA_OUT_EP, OUT_FROM_HOST);
break;

- case MSD_ERROR_UNSUPPORTED_COMMAND:
- ResetSenseData();
+ case MSD_ERROR_UNSUPPORTED_COMMAND:
+ ResetSenseData();
gblSenseData[LUN_INDEX].SenseKey=S_ILLEGAL_REQUEST;
gblSenseData[LUN_INDEX].ASC=ASC_INVALID_COMMAND_OPCODE;
gblSenseData[LUN_INDEX].ASCQ=ASCQ_INVALID_COMMAND_OPCODE;
- if((OldMSD_State == MSD_DATA_OUT) &&
(gblCBW.dCBWDataTransferLength != 0))
- {
- USBStallEndpoint(MSD_DATA_OUT_EP, OUT_FROM_HOST);
+ if((OldMSD_State == MSD_DATA_OUT) && (gblCBW.dCBWDataTransferLength !=
0))
+ {
+ USBStallEndpoint(MSD_DATA_OUT_EP, OUT_FROM_HOST);
}
else
{
- USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST);
- }
- break;
- default: //Shouldn't get hit, don't call MSDErrorHandler() if there is
no error
+ USBStallEndpoint(MSD_DATA_IN_EP, IN_TO_HOST);
+ }
break;
- }//switch(ErrorCase)
-}
+ default: // Shouldn't get hit, don't call MSDErrorHandler() if there is
no error
+ break;
+ }//switch(ErrorCase)
+}



@@ -1709,4 +1708,4 @@
#endif //end of #ifdef USB_USE_MSD
//End of file usb_function_msd.c

-#endif // defined(__dsPIC33E__)
+#endif // defined(__dsPIC33E__)
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/mplab8-template.txt Sat
Apr 25 03:12:54 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/mplab8-template.txt Sun
Jun 14 13:43:17 2015 UTC
@@ -6,7 +6,7 @@
BuildDirPolicy=BuildDirIsProjectDir
dir_src=
dir_bin=
-dir_tmp=build/%%TARGET_BOARD%%
+dir_tmp=%%PROJECT%%.C30/build
dir_sin=..\libVectorMatrix
dir_inc=%%CONFIG%%%%INCLUDES%%
dir_lib=C:\Program Files\Microchip\MPLAB C30\lib
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/pyProjectCreator.py Fri
May 1 02:12:19 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/pyProjectCreator.py Sun
Jun 14 13:43:17 2015 UTC
@@ -30,21 +30,21 @@
for d in defines:
if not d == '':
redefs = redefs + sep + d
-# redefs = redefs.strip(sep)
print "redefs: ", redefs
return redefs

#
# configuration from makefile scripts
#
-#def parse_options_file(filename, option):
-# str = ''
-# with open (filename, "r") as file:
-# data = file.read()
-# match = re.search(r"(^" + option + " .= )(.*$)", data, re.MULTILINE)
-# if match:
-# str = match.group(2)
-# return str
+
+def parse_options_file(filename, option):
+ str = ''
+ with open (filename, "r") as file:
+ data = file.read()
+ match = re.search(r"(^" + option + " .= )(.*$)", data, re.MULTILINE)
+ if match:
+ str = match.group(2)
+ return str

def parse_mk_file(filename, option, list):
with open (filename, "r") as file:
@@ -55,7 +55,14 @@
return list

#
-# EXAMPLE MAKEFILE/OPTIONS FILE:
+# EXAMPLE MAKEFILE/target-* FILE:
+#
+# modules := MatrixPilot MAVLink libDCM
+# incpath := MAVLink/include
+# cfgpath := Config
+
+#
+# EXAMPLE MAKEFILE/device-* FILE:
#
# TOOLCHAIN ?= XC16
# TARGET_TYPE := hex
@@ -89,19 +96,18 @@
file_info = file_info + "file_" + str.format('{:0>3}', file_cnt) + "="
+ filename + '\n'
file_cnt = file_cnt + 1

-def mplab8_project(mcu_type, target_board, root_sep, config_dir, includes,
project, defines):
+def mplab8_project(mcu_type, target_board, prjname, root_sep, config_dir,
includes, project, defines):
config = ''
for e in config_dir:
config = config + root_sep + e + ';'
-
fixdeps = root_sep + config_dir[0]
-# print "fixdeps: ", fixdeps
-
- if not defines == '':
- defines = ';' + defines
-
+ defs = ""
+ for d in defines:
+ if d:
+ defs = defs + "-D" + d + " "
with open (script_path + "mplab8-template.txt", "r") as file:
data = file.read()
+ data = data.replace("%%PROJECT%%", prjname)
data = data.replace("%%DEVICE%%", mcu_type)
data = data.replace("%%CONFIG%%", config)
data = data.replace("%%INCLUDES%%", includes)
@@ -110,9 +116,8 @@
data = data.replace("%%GENERATED_FILES%%", other_files)
data = data.replace("%%OTHER_FILES%%", other_files)
data = data.replace("%%FILE_INFO%%", file_info)
- data = data.replace("%%EXTRA_DEFS%%", defines.replace(";", "
-D").strip())
+ data = data.replace("%%EXTRA_DEFS%%", defs)
data = data.replace("%%FIXDEPS%%", fixdeps)
-
mkdirnotex(project + ".mcp")
with open (project + ".mcp", "w") as file:
print "writing: " + project + ".mcp"
@@ -173,7 +178,10 @@
config = ''
for e in config_dir:
config = config + root_sep + e + ';'
-
+ defs = ""
+ for d in defines:
+ if d:
+ defs = defs + d + " "
with open (script_path + "template.vcxproj", "r") as file:
data = file.read()
data = data.replace("%%CONFIG%%", config)
@@ -181,11 +189,18 @@
data = data.replace("%%INCLUDES%%", includes)
data = data.replace("%%SOURCE_FILES%%", source_files)
data = data.replace("%%HEADER_FILES%%", header_files)
- data = data.replace("%%EXTRA_DEFS%%", defines.strip(";"))
+ data = data.replace("%%EXTRA_DEFS%%", defs)
mkdirnotex(project + ".vcxproj")
with open (project + ".vcxproj", "w") as file:
print "writing: " + project + ".vcxproj"
file.write(data)
+ with open (script_path + "template.sln", "r") as file:
+ data = file.read()
+ data = data.replace("%%PROJECT%%", prjname)
+ mkdirnotex(project + ".sln")
+ with open (project + ".sln", "w") as file:
+ print "writing: " + project + ".sln"
+ file.write(data)

def vs2010_filters(mcu_type, target_board, root_sep, config_dir, filters,
header_files, source_files, project):
with open (script_path + "template.vcxproj.filters", "r") as file:
@@ -195,6 +210,7 @@
data = data.replace("%%HEADER_FILES%%", header_files)
mkdirnotex(project + ".vcxproj.filters")
with open (project + ".vcxproj.filters", "w") as file:
+ print "writing: " + project + ".vcxproj.filters"
file.write(data)

#
@@ -254,7 +270,10 @@
config = ''
for e in config_dir:
config = config + root_sep + e + ';'
-
+ defs = ""
+ for d in defines:
+ if d:
+ defs = defs + d + " "
with open (script_path + "configurations.xml", "r") as file:
data = file.read()
data = data.replace("%%NAME%%", name)
@@ -264,7 +283,7 @@
data = data.replace("%%TARGET_BOARD%%", target_board)
data = data.replace("%%HEADER_FILES%%", header_files)
data = data.replace("%%SOURCE_FILES%%", source_files)
- data = data.replace("%%EXTRA_DEFS%%", defines.strip(";"))
+ data = data.replace("%%EXTRA_DEFS%%", defs)
with open (os.path.join(project_path, "configurations.xml"), "w") as file:
file.write(data)
with open (script_path + "project.xml", "r") as file:
@@ -294,14 +313,15 @@
str = str + "\t\t<Unit filename=\"" + filename.replace("/", "\\")
+ "\" />\n"
return str

-def emBlocks_project(mcu_type, target_board, root_sep, config_dir,
includes, header_files, source_files, project):
+def emBlocks_project(mcu_type, name, target_board, config_dir, defines,
includes, headers, sources, project):
with open (script_path + "template.ebp", "r") as file:
data = file.read()
- data = data.replace("%%PROJECT%%", "MatrixPilot-PX4")
+ data = data.replace("%%PROJECT%%", name + "-" + target_board)
+ data = data.replace("%%DEFINES%%", defines)
data = data.replace("%%INCLUDES%%", includes)
data = data.replace("%%TARGET_BOARD%%", target_board)
- data = data.replace("%%SOURCE_FILES%%", source_files)
- data = data.replace("%%HEADER_FILES%%", header_files)
+ data = data.replace("%%SOURCE_FILES%%", sources)
+ data = data.replace("%%HEADER_FILES%%", headers)
mkdirnotex(project + ".ebp")
with open (project + ".ebp", "w") as file:
print "writing: " + project + ".ebp"
@@ -316,15 +336,14 @@

from optparse import OptionParser
parser = OptionParser("pyProjectCreator.py [options]")
+ parser.add_option("-r", "--root", dest="root", help="project root
path", default=".")
parser.add_option("-n", "--name", dest="name", help="specify the
project name", type="string", default="MatrixPilot", metavar="MatrixPilot")
parser.add_option("-t", "--target", dest="target", help="specify the
target board", type="string", default="UDB5", metavar="UDB5")
parser.add_option("-m", "--mod", dest="modules", help="search path
for module.mk file", default=[], action='append')
parser.add_option("-d", "--def", dest="defines", help="additional
preprocessor defines", default=[], action='append')
parser.add_option("-i", "--inc", dest="includes", help="additional
include files directory", default=[], action='append')
parser.add_option("-c", "--cfg", dest="config", help="specify
configuration files directory", default=[], action='append')
-# parser.add_option("-c", "--cfg", dest="config", help="specify
configuration files directory", default="")
- parser.add_option("-o", "--out", dest="out", help="project files
output path", default="build")
- parser.add_option("-r", "--root", dest="root", help="project root
path", default=".")
+ parser.add_option("-o", "--out", dest="out", help="project files
output path", default="_build")
parser.add_option("-f", "--file", dest="file", help="configuration
file", default="")
(opts, args) = parser.parse_args()

@@ -362,57 +381,52 @@
opts.defines = parse_mk_file(opts.file, "defines", opts.defines)
arch = ''.join(parse_mk_file(opts.file, "CPU", ["dsPIC"]))

-# TODO: perhaps we want to check that the modules list (etc) is not empty..
+ for mod in opts.modules:
+ mod_incs = parse_options_file(opts.root + "/" + mod
+ "/module.mk", "local_inc").split(' ')
+ if not mod_incs == ['']:
+ for mi in mod_incs:
+ opts.includes = opts.includes + [mod + "/" + mi]
+ mod_defs = parse_options_file(opts.root + "/" + mod
+ "/module.mk", "defines").split(' ')
+ if not mod_defs == ['']:
+ for md in mod_defs:
+ if not md == '':
+ opts.defines = opts.defines + [md]

rootsep = "../"
inc_list = [rootsep + str(x) for x in opts.includes]
includes = ';'.join(inc_list)
- opts.defines = ";".join(opts.defines)
filters = ""
-
-
-# cfgname = opts.config[0].replace("/", " ").split(" ")
-# prjname = opts.name + "-" + opts.target + "-" + cfgname[-1]

prjname = opts.name + "-" + opts.target + "-" +
opts.config[0].replace("/", " ").split(" ")[-1]
-
-# prjname = opts.name + "-" + opts.target + "-" +
opts.config[0].replace("/", "-")
-# project = os.path.join(opts.out, opts.name + "-" + opts.target + "-" +
opts.config[0].replace("/", "-"))
project = os.path.join(opts.out, prjname)
-# print "output directory: " + opts.out
print "project: " + prjname

if opts.target == "PX4":
sources = emBlocks_scan_dirs(["*.c", "*.s"], 1, opts.modules)
headers = emBlocks_scan_dirs(["*.h"], 0, opts.config + opts.modules +
["libUDB"])
includes = ""
+ defines = "\t\t\t<Add option=\"-D" + opts.target + "\" />\n"
+ for d in opts.defines:
+ if d:
+ defines = defines + "\t\t\t<Add option=\"-D" + d + "\" />\n"
for inc in inc_list:
includes = includes + "\t\t\t<Add directory=\"" + inc + "\" />\n"
-# project_path = project + ".ebp"
-# print "writing: " + project + ".ebp"
- emBlocks_project(arch, opts.target, rootsep, opts.config, includes,
headers, sources, project)
+ emBlocks_project(arch, opts.name, opts.target, opts.config, defines,
includes, headers, sources, project)
elif opts.target == "SIL":
sources = vs2010_scan_dirs(["*.c"], 1, opts.modules)
headers = vs2010_scan_dirs(["*.h"], 0, opts.config + opts.modules +
["libUDB"])
-# project_path = project + ".vcxproj"
-# print "writing: " + project + ".vcxproj"
vs2010_project(arch, opts.target, rootsep, opts.config, includes,
headers, sources, project, opts.defines, prjname)
sources = vs2010_scan_filter_dirs(["*.c"], 1, opts.modules, "Source
Files\\")
headers = vs2010_scan_filter_dirs(["*.h"], 0, opts.config + opts.modules
+ ["libUDB"], "Header Files\\")
filters = filters + vs2010_make_filter_dirs("Source", source_folders)
filters = filters + vs2010_make_filter_dirs("Header", header_folders)
-# project_path = project + ".vcxproj.filters"
vs2010_filters(arch, opts.target, rootsep, opts.config, filters,
headers, sources, project)
else:
mplab8_scan_dirs("*.c", opts.modules)
mplab8_scan_dirs("*.s", opts.modules)
mplab8_scan_dirs("*.h", opts.config + opts.modules)
-# project_path = project + ".mcp"
-# print "writing: " + project + ".mcp"
- mplab8_project(arch, opts.target, rootsep, opts.config, includes,
project, opts.defines)
+ mplab8_project(arch, opts.target, prjname, rootsep, opts.config,
includes, project, opts.defines)
headers = mplabX_scan_dirs(["*.h", "*.inc"], opts.config + opts.modules)
sources = mplabX_scan_dirs(["*.c", "*.s"], opts.modules)
-# project_path = project + ".X"
-# print "writing: " + project + ".X"
includes = ';'.join(["../" + str(x) for x in inc_list])
mplabX_project(arch, opts.name, opts.target, "../" + rootsep,
opts.config, includes, headers, sources, project + ".X", opts.defines)
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/template.ebp Fri May 1
03:42:52 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/template.ebp Sun Jun 14
13:43:17 2015 UTC
@@ -8,8 +8,8 @@
<Option compiler="armgcc_eb" />
<Build>
<Target title="Debug">
- <Option output="bin\Debug\%%PROJECT%%.elf" />
- <Option object_output="obj\Debug\" />
+ <Option output="EmBlocks\bin\Debug\%%PROJECT%%.elf" />
+ <Option object_output="EmBlocks\obj\Debug\" />
<Option type="0" />
<Option compiler="armgcc_eb" />
<Option projectDeviceOptionsRelation="0" />
@@ -29,8 +29,8 @@
</Linker>
</Target>
<Target title="Release">
- <Option output="bin\Release\%%PROJECT%%.elf" />
- <Option object_output="obj\Release\" />
+ <Option output="EmBlocks\bin\Release\%%PROJECT%%.elf" />
+ <Option object_output="EmBlocks\obj\Release\" />
<Option type="0" />
<Option create_hex="1" />
<Option compiler="armgcc_eb" />
@@ -59,26 +59,9 @@
</Device>
<Compiler>
<Add option="-mfloat-abi=hard" />
- <Add option="-DARM_MATH_CM4" />
- <Add option="-D__FPU_USED" />
- <Add option="-DSTM32F401RE" />
- <Add option="-DSTM32F4XX" />
- <Add option="-DUSE_STDPERIPH_DRIVER" />
- <Add option="-DSTM32F401xE" />
- <Add option="-DPX4" />
<Add option="-fno-strict-aliasing" />
+%%DEFINES%%
%%INCLUDES%%
- <Add directory="..\libSTM\inc" />
- <Add directory="..\libSTM\src" />
- <Add directory="..\libSTM\cmsis" />
- <Add directory="..\libSTM\Drivers" />
- <Add directory="..\libSTM\Drivers\CMSIS\Device\ST\STM32F4xx\Include" />
- <Add directory="..\libSTM\Drivers\CMSIS\Include" />
- <Add directory="..\libSTM\Drivers\STM32F4xx_HAL_Driver\Inc" />
- <Add directory="..\libSTM\Middlewares" />
- <Add
directory="..\libSTM\Middlewares\Third_Party\FreeRTOS\Source\include" />
- <Add
directory="..\libSTM\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS" />
- <Add
directory="..\libSTM\Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F"
/>
</Compiler>
<Linker>
<Add option="-eb_start_files" />
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/Build/template.vcxproj Fri
May 1 02:12:19 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/Build/template.vcxproj Sun Jun
14 13:43:17 2015 UTC
@@ -39,11 +39,11 @@
<PropertyGroup Label="UserMacros" />
<PropertyGroup>
<_ProjectFileVersion>10.0.40219.1</_ProjectFileVersion>
- <OutDir Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">Debug\</OutDir>
- <IntDir Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">Debug\</IntDir>
+ <OutDir Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">MSVC\Debug\</OutDir>
+ <IntDir Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">MSVC\Debug\</IntDir>
<LinkIncremental Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">true</LinkIncremental>
- <OutDir Condition="'$(Configuration)|$(Platform)'=='Release|
Win32'">Release\</OutDir>
- <IntDir Condition="'$(Configuration)|$(Platform)'=='Release|
Win32'">Release\</IntDir>
+ <OutDir Condition="'$(Configuration)|$(Platform)'=='Release|
Win32'">MSVC\Release\</OutDir>
+ <IntDir Condition="'$(Configuration)|$(Platform)'=='Release|
Win32'">MSVC\Release\</IntDir>
<LinkIncremental Condition="'$(Configuration)|$(Platform)'=='Release|
Win32'">false</LinkIncremental>
<TargetExt Condition="'$(Configuration)|$(Platform)'=='Debug|
Win32'">.exe</TargetExt>
</PropertyGroup>
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/LedTest/Config/options.h Wed
May 6 14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/LedTest/Config/options.h Sun
Jun 14 13:43:17 2015 UTC
@@ -236,11 +236,6 @@
#define USE_TELELOG 0
#endif

-// Set this to 1 to enable loading options settings from an initialisation
(ini) file
-#ifndef USE_CONFIGFILE
-#define USE_CONFIGFILE 0
-#endif
-
// Set this to 1 to enable the USB stack on AUAV3
#ifndef USE_USB
#define USE_USB 0
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/LedTest/main.c Fri May 1
02:12:19 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/LedTest/main.c Sun Jun 14
13:43:17 2015 UTC
@@ -20,9 +20,11 @@


#include "../../libUDB/libUDB.h"
-#include "../../libUDB/heartbeat.h"
#include "../../libUDB/mpu6000.h"
+#include "../../libUDB/servoOut.h"
#include "../../libUDB/ADchannel.h"
+#include "../../libUDB/heartbeat.h"
+#include "../../libUDB/eeprom_udb4.h"
#include <libpic30.h>

#if (BOARD_TYPE == UDB4_BOARD)
@@ -51,6 +53,7 @@
#if (BOARD_TYPE != AUAV3_BOARD)
udb_eeprom_init(); // using legacy eeprom driver
#endif
+ DPRINT("MatrixPilot LedTest\r\n");
while (1)
{
udb_run();
@@ -67,77 +70,78 @@
#if (BOARD_TYPE == UDB4_BOARD || BOARD_TYPE == UDB5_BOARD)
case 9:
udb_background_trigger(&udb_background_callback_triggered);
- LED_RED = LED_ON;
+ led_on(LED_RED);
break;
case 8:
- LED_RED = LED_OFF;
- LED_GREEN = LED_ON;
+ led_off(LED_RED);
+ led_on(LED_GREEN);
break;
case 7:
- LED_GREEN = LED_OFF;
- LED_ORANGE = LED_ON;
+ led_off(LED_GREEN);
+ led_on(LED_ORANGE);
break;
case 6:
- LED_ORANGE = LED_OFF;
- LED_BLUE = LED_ON;
+ led_off(LED_ORANGE);
+ led_on(LED_BLUE);
break;
case 5:
- LED_BLUE = LED_OFF;
- LED_RED = LED_ON;
+ led_off(LED_BLUE);
+ led_on(LED_RED);
break;
case 4:
- LED_RED = LED_OFF;
- LED_GREEN = LED_ON;
+ led_off(LED_RED);
+ led_on(LED_GREEN);
break;
case 3:
- LED_GREEN = LED_OFF;
- LED_ORANGE = LED_ON;
+ led_off(LED_GREEN);
+ led_on(LED_ORANGE);
break;
case 2:
- LED_BLUE = LED_ON;
- LED_ORANGE = LED_OFF;
+ led_on(LED_BLUE);
+ led_off(LED_ORANGE);
break;
case 1:
- LED_BLUE = LED_OFF;
+ led_off(LED_BLUE);
udb_a2d_record_offsets();
break;
#elif (BOARD_TYPE == AUAV3_BOARD)
case 9:
udb_background_trigger(&udb_background_callback_triggered);
- LED_BLUE = LED_ON;
+ led_on(LED_BLUE);
break;
case 8:
- LED_BLUE = LED_OFF;
- LED_RED = LED_ON;
+ led_off(LED_BLUE);
+ led_on(LED_RED);
break;
case 7:
- LED_RED = LED_OFF;
- LED_GREEN = LED_ON;
+ led_off(LED_RED);
+ led_on(LED_GREEN);
break;
case 6:
- LED_GREEN = LED_OFF;
- LED_ORANGE = LED_ON;
+ led_off(LED_GREEN);
+ led_on(LED_ORANGE);
break;
case 5:
- LED_ORANGE = LED_OFF;
- LED_BLUE = LED_ON;
+ led_off(LED_ORANGE);
+ led_on(LED_BLUE);
break;
case 4:
- LED_BLUE = LED_OFF;
- LED_RED = LED_ON;
+ led_off(LED_BLUE);
+ led_on(LED_RED);
break;
case 3:
- LED_RED = LED_OFF;
- LED_GREEN = LED_ON;
+ led_off(LED_RED);
+ led_on(LED_GREEN);
break;
case 2:
- LED_GREEN = LED_OFF;
- LED_ORANGE = LED_ON;
+ led_off(LED_GREEN);
+ led_on(LED_ORANGE);
break;
case 1:
- LED_ORANGE = LED_OFF;
+ led_off(LED_ORANGE);
udb_a2d_record_offsets();
break;
+#elif (BOARD_TYPE == PX4_BOARD)
#else
#error "unsupported BOARD_TYPE"
#endif
@@ -189,6 +193,7 @@
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = 3000;
} else if (eepromSuccess == 0 && eepromFailureFlashCount) {
// eeprom failure!
+ DPRINT("eeprom failure!\r\n");
if (udb_heartbeat_counter % 6 == 0) {
udb_led_toggle(LED_RED);
udb_led_toggle(LED_GREEN);
@@ -218,15 +223,15 @@
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 +
accum._.W1);

if ((udb_heartbeat_counter / 600) % 2 == 0) {
- LED_RED = LED_ON;
- LED_ORANGE = ((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL] - 3000) >
RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
- LED_BLUE = ((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) >
RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
- LED_GREEN = ((abs(udb_pwOut[YAW_OUTPUT_CHANNEL] - 3000) >
RATE_THRESHOLD_LED) ? LED_ON : LED_OFF);
+ led_on(LED_RED);
+ ((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ?
led_on(LED_ORANGE) : led_off(LED_ORANGE));
+ ((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ?
led_on(LED_BLUE) : led_off(LED_BLUE));
+ ((abs(udb_pwOut[YAW_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ?
led_on(LED_GREEN) : led_off(LED_GREEN));
} else {
- LED_RED = LED_OFF;
- LED_ORANGE = ((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
- LED_BLUE = ((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
- LED_GREEN = ((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? LED_ON : LED_OFF);
+ led_off(LED_RED);
+ ((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
+ ((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
+ ((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) >
ACCEL_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
}
}
}
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-I2C1.c Wed
Apr 15 23:54:37 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-I2C1.c Sun
Jun 14 13:43:17 2015 UTC
@@ -23,7 +23,6 @@
#include "../../libUDB/libUDB.h"
#include "../../libUDB/interrupt.h"
#include "../../libUDB/I2C.h"
-#include "../../libUDB/NV_memory.h"
#include "../../libUDB/events.h"

#if (USE_I2C1_DRIVER == 1)
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-serial.c
Wed May 6 14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-serial.c
Sun Jun 14 13:43:17 2015 UTC
@@ -27,7 +27,7 @@


//////////////////////////////////////////////////////////
-// GPS and Telemetry
+// GPS Serial input/output emulation
//////////////////////////////////////////////////////////

void udb_gps_set_rate(int32_t rate)
@@ -63,6 +63,9 @@
}
}

+//////////////////////////////////////////////////////////
+// Telemetry Serial input/output emulation
+//////////////////////////////////////////////////////////

void udb_serial_set_rate(int32_t rate)
{
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.c Wed
May 6 14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.c Sun
Jun 14 13:43:17 2015 UTC
@@ -471,7 +471,6 @@
(abs(udb_magFieldBody[1]) < MAGNETICMAXIMUM) &&
(abs(udb_magFieldBody[2]) < MAGNETICMAXIMUM))
{
-// udb_magnetometer_callback();
if (magnetometer_callback != NULL)
{
magnetometer_callback();
=======================================
--- /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.h Wed
May 6 14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/Tools/MatrixPilot-SIL/SIL-udb.h Sun
Jun 14 13:43:17 2015 UTC
@@ -64,12 +64,12 @@
extern volatile uint32_t trap_source;
extern volatile uint16_t osc_fail_count;

-void udb_magnetometer_callback(void);
uint16_t get_reset_flags(void);
void sil_reset(void);

uint16_t get_current_milliseconds(void);
void sleep_milliseconds(uint16_t ms);

+void sil_telemetry_input(uint8_t* buffer, int32_t bytesRead);

#endif
=======================================
--- /branches/MatrixPilot_helicalTurns/device-AUAV3.mk Sat Apr 18 15:53:51
2015 UTC
+++ /branches/MatrixPilot_helicalTurns/device-AUAV3.mk Sun Jun 14 13:43:17
2015 UTC
@@ -9,4 +9,4 @@
incpath += Microchip Microchip/Include libVectorMatrix
#cfgpath := Config

-defines += USE_USB USE_CDC USE_MSD USE_FILESYS
+#defines += USE_USB USE_CDC USE_MSD USE_FILESYS
=======================================
--- /branches/MatrixPilot_helicalTurns/libDCM/estAirspeed.c Wed May 6
14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/libDCM/estAirspeed.c Sun Jun 14
13:43:17 2015 UTC
@@ -41,7 +41,7 @@
* lacks the air density compensation. Use the calc_true_airspeed
functions to get
* the true airspeed.
*
- * @parem pressure_front pressure inside the pitot/prandl tube
+ * @param pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return indicated airspeed in m/s
@@ -56,7 +56,7 @@
*
* Please note that the true airspeed is NOT the groundspeed, because of
the effects of wind
*
- * @parem speed current indicated airspeed
+ * @param speed current indicated airspeed
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
@@ -71,7 +71,7 @@
*
* Please note that the true airspeed is NOT the groundspeed, because of
the effects of wind
*
- * @parem pressure_front pressure inside the pitot/prandl tube
+ * @param pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
=======================================
--- /branches/MatrixPilot_helicalTurns/libUDB/ConfigPX4.h Wed May 6
14:48:43 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/libUDB/ConfigPX4.h Sun Jun 14
13:43:17 2015 UTC
@@ -61,8 +61,8 @@
#define ZACCEL_SIGN +

// Max inputs and outputs
-#define MAX_INPUTS 8
-#define MAX_OUTPUTS 8
+#define MAX_INPUTS 6
+#define MAX_OUTPUTS 6

// LED pins
#define LED_RED 0
@@ -89,22 +89,29 @@

// Input Capture module input pins: PINX is the port pin, RPINX is used
for PPS
// PINX must match RPINX for radionIn_auav3 to work properly
-#define IC_PIN1 _RD0
-#define IC_RPIN1 64
-#define IC_PIN2 _RD8
-#define IC_RPIN2 72
-#define IC_PIN3 _RD11
-#define IC_RPIN3 75
-#define IC_PIN4 _RA15
-#define IC_RPIN4 31
-#define IC_PIN5 _RA5
-#define IC_RPIN5 21
-#define IC_PIN6 _RA14
-#define IC_RPIN6 30
-#define IC_PIN7 _RA4
-#define IC_RPIN7 20
-#define IC_PIN8 _RF8
-#define IC_RPIN8 104
+//#define IC_PIN1 _RD0
+//#define IC_RPIN1 64
+//#define IC_PIN2 _RD8
+//#define IC_RPIN2 72
+//#define IC_PIN3 _RD11
+//#define IC_RPIN3 75
+//#define IC_PIN4 _RA15
+//#define IC_RPIN4 31
+//#define IC_PIN5 _RA5
+//#define IC_RPIN5 21
+//#define IC_PIN6 _RA14
+//#define IC_RPIN6 30
+//#define IC_PIN7 _RA4
+//#define IC_RPIN7 20
+//#define IC_PIN8 _RF8
+//#define IC_RPIN8 104
+
+#define IC_PIN1 GPIOA->IDR & (1 << 0) //PA0
+#define IC_PIN2 GPIOA->IDR & (1 << 1) //PA1
+#define IC_PIN3 GPIOB->IDR & (1 << 6) //PB6
+#define IC_PIN4 GPIOB->IDR & (1 << 7) //PB7
+#define IC_PIN5 GPIOB->IDR & (1 << 8) //PB8
+#define IC_PIN6 GPIOB->IDR & (1 << 9) //PB9

// OC1:8 PWM module output pins
// PINX must match RPINX
=======================================
--- /branches/MatrixPilot_helicalTurns/libUDB/background.c Sat Apr 18
15:53:51 2015 UTC
+++ /branches/MatrixPilot_helicalTurns/libUDB/background.c Sun Jun 14
13:43:17 2015 UTC
@@ -178,7 +178,7 @@
indicate_loading_inter;
interrupt_save_set_corcon;
_T6IF = 0; // clear the interrupt
- if (callback_fptr_1) callback_fptr_1(); // was called pulse() /
heartbeat_pulse()
+ if (callback_fptr_1) callback_fptr_1(); // was called pulse() or
heartbeat_pulse()
interrupt_restore_corcon;
}