rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merge branch 'revo-mini' into revo

Browse Source
This commit is contained in:
James Cotton 2012-09-12 02:30:10 -05:00
commit 13e7733e5f
197 changed files with 17508 additions and 7030 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Makefile
View File

@ -654,7 +654,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
endef
ALL_BOARDS := coptercontrol pipxtreme revolution simposix osd
ALL_BOARDS := coptercontrol pipxtreme revolution revomini simposix osd
ALL_BOARDS_BU := coptercontrol pipxtreme simposix
# SimPosix only builds on Linux so drop it from the list for
@ -674,6 +674,7 @@ endif
coptercontrol_friendly := CopterControl
pipxtreme_friendly := PipXtreme
revolution_friendly := Revolution
revomini_friendly := RevoMini
simposix_friendly := SimPosix
osd_friendly := OSD
@ -681,13 +682,7 @@ osd_friendly := OSD
coptercontrol_short := 'cc '
pipxtreme_short := 'pipx'
revolution_short := 'revo'
simposix_short := 'posx'
osd_short := 'osd '
# Short hames of each board (used to display board name in parallel builds)
coptercontrol_short := 'cc '
pipxtreme_short := 'pipx'
revolution_short := 'revo'
revomini_short := 'rm'
simposix_short := 'posx'
osd_short := 'osd '

140
androidgcs/AndroidManifest.xml
View File

@ -1,95 +1,71 @@
<?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="org.openpilot.androidgcs" android:versionCode="1"
android:versionName="1.0">
<uses-sdk android:minSdkVersion="14" />
package="org.openpilot.androidgcs"
android:versionCode="1"
android:versionName="1.0" >
<uses-permission android:name="android.permission.INTERNET" />
<uses-sdk android:minSdkVersion="14" />
<uses-permission android:name="android.permission.BLUETOOTH" />
<uses-permission android:name="android.permission.BLUETOOTH_ADMIN" />
<uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>
<uses-feature android:name="android.hardware.usb.host" />
<application android:icon="@drawable/ic_logo" android:label="@string/app_name" android:theme="@android:style/Theme.Holo">
<!-- for map overlay -->
<uses-library android:name="com.google.android.maps" />
<uses-feature android:glEsVersion="0x00020000" /> <!-- OpenGL min requierements (2.0) -->
<!-- Object browser - main activity at the moment -->
<activity android:name="HomePage" android:label="@string/app_name">
<intent-filter>
<action android:name="android.intent.action.MAIN" />
<category android:name="android.intent.category.LAUNCHER" />
</intent-filter>
<!-- <intent-filter> -->
<!-- <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" /> -->
<!-- </intent-filter> -->
<uses-permission android:name="android.permission.INTERNET" />
<uses-permission android:name="android.permission.BLUETOOTH" />
<uses-permission android:name="android.permission.BLUETOOTH_ADMIN" />
<uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
<!-- <meta-data android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" -->
<!-- android:resource="@xml/device_filter" /> -->
</activity>
<uses-feature android:name="android.hardware.usb.host" />
<activity android:name="ObjectBrowser" android:label="@string/object_browser_name" />
<activity android:name="PfdActivity" android:label="PFD" />
<activity android:name="Controller" android:label="@string/controller_name" />
<activity android:name="Preferences" android:label="@string/preference_title" />
<activity android:name="UAVLocation" android:label="@string/location_name" />
<activity android:name="SystemAlarmActivity" android:label="System Alarms" />
<activity android:name="TuningActivity" android:label="Tuning" />
<activity android:name="ObjectEditor" android:label="ObjectEditor"
android:theme="@android:style/Theme.Dialog" />
<activity android:name="Logger" android:label="Logger"
android:theme="@android:style/Theme.Dialog" />
<activity
android:name="FragmentTester"
android:label="FragmentTester" />
<application
android:icon="@drawable/ic_logo"
android:label="@string/app_name"
android:theme="@android:style/Theme.Holo" >
<!-- for map overlay -->
<uses-library android:name="com.google.android.maps" />
<!-- Object browser - main activity at the moment -->
<activity
android:name="org.openpilot.androidgcs.HomePage"
android:label="@string/app_name" >
<intent-filter>
<action android:name="android.intent.action.MAIN" />
<category android:name="android.intent.category.LAUNCHER" />
</intent-filter>
<!-- <intent-filter> -->
<!-- <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" /> -->
<!-- </intent-filter> -->
<!-- <meta-data android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" -->
<!-- android:resource="@xml/device_filter" /> -->
</activity>
<activity
android:name="ObjectBrowser"
android:label="@string/object_browser_name" />
<activity
android:name="PfdActivity"
android:label="PFD" />
<activity
android:name="Controller"
android:label="@string/controller_name" />
<activity
android:name="Preferences"
android:label="@string/preference_title" />
<activity
android:name="UAVLocation"
android:label="@string/location_name" />
<activity
android:name="SystemAlarmActivity"
android:label="System Alarms" />
<activity
android:name="ObjectEditor"
android:label="ObjectEditor"
android:theme="@android:style/Theme.Dialog" />
<activity
android:name="Logger"
android:label="Logger"
android:theme="@android:style/Theme.Dialog" />
<activity
android:name="OsgViewer"
android:label="3DView" />
<receiver android:name="TelemetryWidget" >
<intent-filter>
<action android:name="android.appwidget.action.APPWIDGET_UPDATE" />
</intent-filter>
<intent-filter>
<action android:name="org.openpilot.intent.action.CONNECTED" />
<action android:name="org.openpilot.intent.action.DISCONNECTED" />
</intent-filter>
<meta-data
android:name="android.appwidget.provider"
android:resource="@xml/telemetry_widget_info" />
</receiver>
<service android:name="org.openpilot.androidgcs.telemetry.OPTelemetryService" >
</service>
</application>
<receiver android:name="TelemetryWidget">
<intent-filter>
<action android:name="android.appwidget.action.APPWIDGET_UPDATE" />
</intent-filter>
<intent-filter>
<action android:name="org.openpilot.intent.action.CONNECTED" />
<action android:name="org.openpilot.intent.action.DISCONNECTED" />
</intent-filter>
<meta-data android:name="android.appwidget.provider"
android:resource="@xml/telemetry_widget_info" />
</receiver>
<service android:name="org.openpilot.androidgcs.telemetry.OPTelemetryService"></service>
</application>
</manifest>

BIN
androidgcs/res/drawable-hdpi/ic_logging.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 823 B

After

Width:  |  Height:  |  Size: 849 B

BIN
androidgcs/res/drawable-hdpi/ic_tuning.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.9 KiB

BIN
androidgcs/res/drawable-hdpi/ic_uav.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 800 B

After

Width:  |  Height:  |  Size: 797 B

BIN
androidgcs/res/drawable-hdpi/im_pfd_fixed.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.4 KiB

BIN
androidgcs/res/drawable-hdpi/im_pfd_horizon.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.3 KiB

After

Width:  |  Height:  |  Size: 9.7 KiB

BIN
androidgcs/res/drawable-hdpi/im_pfd_reticule.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

138
androidgcs/res/layout-w600dp/gcs_home.xml Normal file
View File

@ -0,0 +1,138 @@
<?xml version="1.0" encoding="utf-8"?>
<GridLayout xmlns:android="http://schemas.android.com/apk/res/android"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentLeft="true"
android:layout_gravity="center_horizontal" >
<Button
android:id="@+id/launch_controller"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="3"
android:layout_gravity="right"
android:layout_row="1"
android:layout_rowSpan="4"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_controller"
android:text="@string/controller_name" />
<Button
android:id="@+id/launch_object_browser"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="0"
android:layout_gravity="right|bottom"
android:layout_row="2"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_browser"
android:text="@string/object_browser_name" />
<Button
android:id="@+id/launch_location"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="2"
android:layout_gravity="right|bottom"
android:layout_row="2"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_map"
android:text="@string/location_name" />
<Button
android:id="@+id/launch_tuning"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="4"
android:layout_gravity="right|bottom"
android:layout_row="2"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_tuning"
android:text="@string/tuning" />
<Button
android:id="@+id/launch_logger"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="3"
android:layout_gravity="left|bottom"
android:layout_row="3"
android:layout_rowSpan="3"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_logging"
android:text="@string/logger_name" />
<Button
android:id="@+id/launch_alarms"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="0"
android:layout_gravity="left"
android:layout_row="5"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/alarms" />
<Button
android:id="@+id/launch_pfd"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="2"
android:layout_gravity="left"
android:layout_row="5"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_pfd"
android:text="@string/pfd_name" />
<Button
android:id="@+id/launch_osgViewer"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="0"
android:layout_gravity="right"
android:layout_row="9"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/_3dview" />
<Button
android:id="@+id/launch_tester"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="2"
android:layout_row="9"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/tester" />
<Space
android:layout_width="1dp"
android:layout_height="32dp"
android:layout_column="0"
android:layout_row="0" />
<Space
android:layout_width="1dp"
android:layout_height="47dp"
android:layout_column="0"
android:layout_row="4" />
<Space
android:layout_width="1dp"
android:layout_height="47dp"
android:layout_column="0"
android:layout_row="8" />
<Space
android:layout_height="15dp"
android:layout_column="1" />
<Space
android:layout_width="100dp"
android:layout_height="2dp"
android:layout_row="6" />
</GridLayout>

103
androidgcs/res/layout-w600dp/tuning.xml Normal file
View File

@ -0,0 +1,103 @@
<?xml version="1.0" encoding="utf-8"?>
<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:setting_attributes="http://schemas.android.com/apk/res/org.openpilot.androidgcs"
android:layout_width="match_parent"
android:layout_height="match_parent"
android:orientation="vertical" >
<Button
android:id="@+id/saveBtn"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentBottom="true"
android:layout_alignParentRight="true"
android:text="@string/save" />
<Button
android:id="@+id/applyBtn"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentBottom="true"
android:layout_toLeftOf="@+id/saveBtn"
android:text="@string/apply" />
<LinearLayout
android:id="@+id/linearLayout3"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_alignParentLeft="true"
android:layout_alignParentTop="true" >
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollRateKp"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="0.01"
setting_attributes:setting_name="Roll Rate Kp" >
</org.openpilot.androidgcs.views.ScrollBarView>
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchRateKp"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="0.01"
setting_attributes:setting_name="Pitch Rate Kp" >
</org.openpilot.androidgcs.views.ScrollBarView>
</LinearLayout>
<LinearLayout
android:id="@+id/linearLayout1"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_alignParentLeft="true"
android:layout_below="@+id/linearLayout3" >
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollRateKi"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="0.05"
setting_attributes:setting_name="Roll Rate Ki" >
</org.openpilot.androidgcs.views.ScrollBarView>
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchRateKi"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="0.05"
setting_attributes:setting_name="Pitch Rate Ki" >
</org.openpilot.androidgcs.views.ScrollBarView>
</LinearLayout>
<LinearLayout
android:id="@+id/linearLayout2"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_alignParentLeft="true"
android:layout_below="@+id/linearLayout1" >
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollKp"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="5"
setting_attributes:setting_name="Roll Kp" >
</org.openpilot.androidgcs.views.ScrollBarView>
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchKp"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_weight="0.5"
setting_attributes:max_value="5"
setting_attributes:setting_name="Pitch Kp" >
</org.openpilot.androidgcs.views.ScrollBarView>
</LinearLayout>
</RelativeLayout>

41
androidgcs/res/layout/gcs_home.xml
View File

@ -81,28 +81,37 @@
android:id="@+id/launch_tester"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_row="8"
android:layout_column="0"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/tester" />
<Button
android:id="@+id/launch_osgViewer"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="1"
android:layout_gravity="right"
android:layout_row="8"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/_3dview" />
<Button
android:id="@+id/launch_tuning"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="0"
android:layout_gravity="right"
android:layout_row="6"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/tester" />
android:drawableTop="@drawable/ic_tuning"
android:text="@string/tuning" />
<Button
android:id="@+id/launch_osgViewer"
android:layout_width="132dp"
android:layout_height="wrap_content"
android:layout_column="1"
android:layout_gravity="right"
android:layout_row="6"
android:layout_rowSpan="2"
android:background="@android:color/transparent"
android:drawableTop="@drawable/ic_alarms"
android:text="@string/_3dview" />
<Space
android:layout_width="1dp"
android:layout_height="32dp"

49
androidgcs/res/layout/object_browser.xml
View File

@ -23,14 +23,14 @@
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:checked="true"
android:text="Settings" />
android:text="@string/settings" />
<CheckBox
android:id="@+id/dataCheck"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:checked="true"
android:text="Data" />
android:text="@string/data" />
</LinearLayout>
@ -56,7 +56,7 @@
android:layout_height="fill_parent"
android:background="#FFFFFFFF" />
<LinearLayout
<RelativeLayout
android:layout_width="fill_parent"
android:layout_height="fill_parent"
android:layout_weight="1"
@ -66,40 +66,29 @@
android:id="@+id/object_information"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentTop="true"
android:layout_marginLeft="12dp"
android:layout_marginTop="12dp"
android:textAppearance="?android:attr/textAppearanceMedium" />
<Space
android:layout_width="match_parent"
android:layout_height="wrap_content" />
<Button
android:id="@+id/editButton"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentBottom="true"
android:layout_alignParentRight="true"
android:text="@string/edit" />
<Space
android:layout_width="match_parent"
android:layout_height="wrap_content" />
<Button
android:id="@+id/object_load_button"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignParentBottom="true"
android:layout_toLeftOf="@+id/editButton"
android:text="@string/load" />
<LinearLayout
android:layout_width="match_parent"
android:layout_height="wrap_content" >
</RelativeLayout>
<Button
android:id="@+id/object_load_button"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:text="Load" />
<Button
android:id="@+id/editButton"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_gravity="right"
android:layout_marginBottom="32dp"
android:layout_marginRight="12dp"
android:text="Edit" />
</LinearLayout>
</LinearLayout>
</LinearLayout>
</LinearLayout>

83
androidgcs/res/layout/tuning.xml Normal file
View File

@ -0,0 +1,83 @@
<?xml version="1.0" encoding="utf-8"?>
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:setting_attributes="http://schemas.android.com/apk/res/org.openpilot.androidgcs"
android:layout_width="match_parent"
android:layout_height="match_parent"
android:orientation="vertical" >
<ScrollView
android:layout_width="match_parent"
android:layout_height="0dp"
android:layout_weight="1"
android:orientation="vertical" >
<LinearLayout
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:orientation="vertical" >
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollRateKp"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Roll Rate Kp"
setting_attributes:max_value="0.01" />
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchRateKp"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Pitch Rate Kp"
setting_attributes:max_value="0.01" />
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollRateKi"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Roll Rate Ki"
setting_attributes:max_value="0.05" />
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchRateKi"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Pitch Rate Ki"
setting_attributes:max_value="0.05" />
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/rollKp"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Roll Kp"
setting_attributes:max_value="5" />
<org.openpilot.androidgcs.views.ScrollBarView
android:id="@+id/pitchKp"
android:layout_width="match_parent"
android:layout_height="wrap_content"
setting_attributes:setting_name="Pitch Kp"
setting_attributes:max_value="5" />
</LinearLayout>
</ScrollView>
<LinearLayout
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:gravity="right" >
<Button
android:id="@+id/applyBtn"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:text="@string/apply" />
<Button
android:id="@+id/saveBtn"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:text="@string/save" />
</LinearLayout>
</LinearLayout>

8
androidgcs/res/values/arrays.xml
View File

@ -14,4 +14,12 @@
<item>3</item>
<item>4</item>
</string-array>
<string-array name="controllerTypeArray">
<item>Mode 1</item>
<item>Mode 2</item>
</string-array>
<string-array name="controllerTypeValues">
<item>1</item>
<item>2</item>
</string-array>
</resources>

7
androidgcs/res/values/attrs.xml Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="utf-8"?>
<resources>
<declare-styleable name="setting_attributes">
<attr name="setting_name" format="string"/>
<attr name="max_value" format="float"/>
</declare-styleable>
</resources>

7
androidgcs/res/values/strings.xml
View File

@ -34,4 +34,11 @@
<string name="rxrate">RxRate: </string>
<string name="tester">Tester</string>
<string name="_3dview">3DView</string>
<string name="tuning">Tuning</string>
<string name="apply">Apply</string>
<string name="save">Save</string>
<string name="settings">Settings</string>
<string name="data">Data</string>
<string name="edit">Edit</string>
<string name="load">Load</string>
</resources>

10
androidgcs/res/xml/controller_preferences.xml Normal file
View File

@ -0,0 +1,10 @@
<?xml version="1.0" encoding="utf-8"?>
<PreferenceScreen xmlns:android="http://schemas.android.com/apk/res/android" >
<PreferenceCategory android:title="Controller Settings">
<ListPreference android:title="Controller Mode"
android:key="controller_type" android:summary="Select whether you want the virtual controller to use mode 1 or mode 2 convention"
android:defaultValue="2"
android:entries="@array/controllerTypeArray"
android:entryValues="@array/controllerTypeValues"/>
</PreferenceCategory>
</PreferenceScreen>

105
androidgcs/src/org/openpilot/androidgcs/AttitudeView.java
View File

@ -56,29 +56,64 @@ public class AttitudeView extends View {
}
@Override protected void onMeasure(int widthMeasureSpec, int heightMeasureSpec) {
int measuredWidth = measure(widthMeasureSpec);
int measuredHeight = measure(heightMeasureSpec);
int d = Math.min(measuredWidth, measuredHeight);
setMeasuredDimension(d/2, d/2);
}
/**
* @see android.view.View#measure(int, int)
*/
@Override
protected void onMeasure(int widthMeasureSpec, int heightMeasureSpec) {
setMeasuredDimension(measureWidth(widthMeasureSpec),
measureHeight(heightMeasureSpec));
}
private int measure(int measureSpec) {
int result = 0;
// Decode the measurement specifications.
int specMode = MeasureSpec.getMode(measureSpec);
int specSize = MeasureSpec.getSize(measureSpec);
/**
* Determines the height of this view
* @param measureSpec A measureSpec packed into an int
* @return The height of the view, honoring constraints from measureSpec
*/
private int measureHeight(int measureSpec) {
int result = 0;
int specMode = MeasureSpec.getMode(measureSpec);
int specSize = MeasureSpec.getSize(measureSpec);
if (specMode == MeasureSpec.UNSPECIFIED) { // Return a default size of 200 if no bounds are specified.
result = 200;
} else {
// As you want to fill the available space
// always return the full available bounds.
result = specSize;
}
return result;
}
if (specMode == MeasureSpec.EXACTLY) {
// We were told how big to be
result = specSize;
} else {
// Measure the text (beware: ascent is a negative number)
result = 1600;
if (specMode == MeasureSpec.AT_MOST) {
// Respect AT_MOST value if that was what is called for by measureSpec
result = Math.min(result, specSize);
}
}
return result;
}
/**
* Determines the width of this view
* @param measureSpec A measureSpec packed into an int
* @return The width of the view, honoring constraints from measureSpec
*/
private int measureWidth(int measureSpec) {
int result = 0;
int specMode = MeasureSpec.getMode(measureSpec);
int specSize = MeasureSpec.getSize(measureSpec);
if (specMode == MeasureSpec.EXACTLY) {
// We were told how big to be
result = specSize;
} else {
// Measure the text
result = 800;
if (specMode == MeasureSpec.AT_MOST) {
// Respect AT_MOST value if that was what is called for by measureSpec
result = Math.min(result, specSize);
}
}
return result;
}
private float roll;
public void setRoll(double roll) {
@ -101,25 +136,35 @@ public class AttitudeView extends View {
canvas.save();
canvas.rotate(-roll, PX / 2, PY / 2);
canvas.save();
canvas.translate(0, pitch * DEG_TO_PX);
Drawable horizon = getContext().getResources().getDrawable(
R.drawable.im_pfd_horizon);
Drawable reticule = getContext().getResources().getDrawable(
R.drawable.im_pfd_reticule);
Drawable fixed = getContext().getResources().getDrawable(
R.drawable.im_pfd_fixed);
// Starting with a square image, want to size it equally
double margin = 0.2;
int screenSize = Math.min(PX, PY);
int imageHalfSize = (int) ((screenSize + screenSize * margin) / 2);
// This puts the image at the center of the PFD canvas (after it was
// translated)
double margin = 0.5;
horizon.setBounds( (int) (-margin * PX), (int) (-margin * PY), (int) ((1 + margin) * PX), (int) ((1+margin) *PY));
horizon.setBounds( PX/2 - imageHalfSize, PY/2 - imageHalfSize, PX/2 + imageHalfSize, PY/2 + imageHalfSize);
horizon.draw(canvas);
canvas.restore();
canvas.drawLine(0, 0, PX, 0, markerPaint);
canvas.drawLine(0, 0, 0, PY, markerPaint);
canvas.drawLine(PX, 0, PX, PY, markerPaint);
canvas.drawLine(0, PY, PX, PY, markerPaint);
// Draw the overlay that only rolls
reticule.setBounds( PX/2 - imageHalfSize, PY/2 - imageHalfSize, PX/2 + imageHalfSize, PY/2 + imageHalfSize);
reticule.draw(canvas);
canvas.restore();
canvas.drawLine(0,PY/2,PX,PY/2,markerPaint);
canvas.drawLine(PX/2,0,PX/2,PY,markerPaint);
}
// Draw the overlay that never moves
fixed.setBounds( PX/2 - imageHalfSize, PY/2 - imageHalfSize, PX/2 + imageHalfSize, PY/2 + imageHalfSize);
fixed.draw(canvas);
}
}

5
androidgcs/src/org/openpilot/androidgcs/BluetoothDevicePreference.java
View File

@ -44,10 +44,7 @@ public class BluetoothDevicePreference extends ListPreference {
Set<BluetoothDevice> pairedDevices = bta.getBondedDevices();
CharSequence[] entries = new CharSequence[pairedDevices.size()];
CharSequence[] entryValues = new CharSequence[pairedDevices.size()];
if (pairedDevices.size() == 0) {
entries[0] = "No Devices";
entryValues[0] = "";
} else {
if (pairedDevices.size() > 0) {
int i = 0;
for (BluetoothDevice dev : pairedDevices) {
entries[i] = dev.getName();

119
androidgcs/src/org/openpilot/androidgcs/Controller.java
View File

@ -34,7 +34,9 @@ import org.openpilot.uavtalk.UAVDataObject;
import org.openpilot.uavtalk.UAVObject;
import org.openpilot.uavtalk.UAVObjectField;
import android.content.SharedPreferences;
import android.os.Bundle;
import android.preference.PreferenceManager;
import android.util.Log;
import android.widget.TextView;
import android.widget.Toast;
@ -76,7 +78,7 @@ public class Controller extends ObjectManagerActivity {
@Override
public void update(Observable observable, Object data) {
// Once we have updated settings we can active the GCS receiver mode
Log.d(TAG,"Got update from settings");
if (DEBUG) Log.d(TAG,"Got update from settings");
UAVDataObject manualControlSettings = (UAVDataObject) objMngr.getObject("ManualControlSettings");
if(manualControlSettings != null) {
manualControlSettings.removeUpdatedObserver(this);
@ -89,7 +91,27 @@ public class Controller extends ObjectManagerActivity {
void onOPConnected() {
super.onOPConnected();
Log.d(TAG, "onOPConnected()");
if (DEBUG) Log.d(TAG, "onOPConnected()");
DualJoystickView joystick = (DualJoystickView) findViewById(R.id.dualjoystickView);
joystick.setMovementConstraint(JoystickView.CONSTRAIN_BOX);
joystick.setMovementRange((int)MOVEMENT_RANGE, (int)MOVEMENT_RANGE);
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(this);
int mode = Integer.decode(prefs.getString("controller_type", "1"));
switch(mode) {
case 1:
if (DEBUG) Log.d(TAG, "Mode1 connected");
joystick.setOnJostickMovedListener(mode1_left, mode1_right);
break;
case 2:
if (DEBUG) Log.d(TAG, "Mode2 connected");
joystick.setOnJostickMovedListener(mode2_left, mode2_right);
break;
default:
Log.e(TAG, "Unknown controller type");
return;
}
// Subscribe to updates from ManualControlCommand and show the values for crude feedback
UAVDataObject manualControl = (UAVDataObject) objMngr.getObject("ManualControlCommand");
@ -100,39 +122,6 @@ public class Controller extends ObjectManagerActivity {
manualSettings.addUpdatedObserver(settingsUpdated);
manualSettings.updateRequested();
final double MOVEMENT_RANGE = 50.0;
DualJoystickView joystick = (DualJoystickView) findViewById(R.id.dualjoystickView);
joystick.setMovementConstraint(JoystickView.CONSTRAIN_BOX);
joystick.setMovementRange((int)MOVEMENT_RANGE, (int)MOVEMENT_RANGE);
// Hardcode a Mode 1 listener for now
joystick.setOnJostickMovedListener(new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
pitch = tilt / MOVEMENT_RANGE;
yaw = pan / MOVEMENT_RANGE;
leftJoystickHeld = true;
}
@Override
public void OnReleased() { leftJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
}, new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
throttle = (-tilt + (MOVEMENT_RANGE -5)) / (MOVEMENT_RANGE - 5);
throttle *= 0.5;
if (throttle < 0)
throttle = -1;
roll = pan / MOVEMENT_RANGE;
rightJoystickHeld = true;
}
@Override
public void OnReleased() { rightJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
});
//! This timer task actually periodically sends updates to the UAV
TimerTask controllerTask = new TimerTask() {
@Override
@ -248,4 +237,64 @@ public class Controller extends ObjectManagerActivity {
return (float) (neutral + (neutral - CHANNEL_MIN) * in);
}
private final JoystickMovedListener mode1_left = new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
pitch = tilt / MOVEMENT_RANGE;
yaw = pan / MOVEMENT_RANGE;
leftJoystickHeld = true;
}
@Override
public void OnReleased() { leftJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
};
private final JoystickMovedListener mode1_right = new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
throttle = (-tilt + (MOVEMENT_RANGE -5)) / (MOVEMENT_RANGE - 5);
throttle *= 0.5;
if (throttle < 0)
throttle = -1;
roll = pan / MOVEMENT_RANGE;
rightJoystickHeld = true;
}
@Override
public void OnReleased() { rightJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
};
private final JoystickMovedListener mode2_left = new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
throttle = (-tilt + (MOVEMENT_RANGE -5)) / (MOVEMENT_RANGE - 5);
throttle *= 0.5;
if (throttle < 0)
throttle = -1;
yaw = pan / MOVEMENT_RANGE;
leftJoystickHeld = true;
}
@Override
public void OnReleased() { leftJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
};
private final JoystickMovedListener mode2_right = new JoystickMovedListener() {
@Override
public void OnMoved(int pan, int tilt) {
pitch = tilt / MOVEMENT_RANGE;
roll = pan / MOVEMENT_RANGE;
rightJoystickHeld = true;
}
@Override
public void OnReleased() { rightJoystickHeld = false; throttle = -1; updated = true; }
@Override
public void OnReturnedToCenter() { }
};
final double MOVEMENT_RANGE = 50.0;
}

8
androidgcs/src/org/openpilot/androidgcs/HomePage.java
View File

@ -100,6 +100,14 @@ public class HomePage extends ObjectManagerActivity {
}
});
Button tuning = (Button) findViewById(R.id.launch_tuning);
tuning.setOnClickListener(new OnClickListener() {
@Override
public void onClick(View arg0) {
startActivity(new Intent(HomePage.this, TuningActivity.class));
}
});
}
}

1
androidgcs/src/org/openpilot/androidgcs/Preferences.java
View File

@ -42,6 +42,7 @@ public class Preferences extends PreferenceActivity {
super.onCreate(savedInstanceState);
// Load the preferences from an XML resource
addPreferencesFromResource(R.xml.preferences);
addPreferencesFromResource(R.xml.controller_preferences);
}
}
}

47
androidgcs/src/org/openpilot/androidgcs/TuningActivity.java Normal file
View File

@ -0,0 +1,47 @@
package org.openpilot.androidgcs;
import org.openpilot.androidgcs.util.SmartSave;
import org.openpilot.androidgcs.views.ScrollBarView;
import org.openpilot.uavtalk.UAVDataObject;
import android.os.Bundle;
import android.util.Log;
import android.widget.Button;
public class TuningActivity extends ObjectManagerActivity {
private final String TAG = TuningActivity.class.getSimpleName();
private final boolean DEBUG = false;
private SmartSave smartSave;
/** Called when the activity is first created. */
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.tuning);
}
@Override
void onOPConnected() {
super.onOPConnected();
if (DEBUG) Log.d(TAG, "onOPConnected()");
// Subscribe to updates from ManualControlCommand and show the values for crude feedback
UAVDataObject stabilizationSettings = (UAVDataObject) objMngr.getObject("StabilizationSettings");
smartSave = new SmartSave(objMngr, stabilizationSettings,
(Button) findViewById(R.id.saveBtn),
(Button) findViewById(R.id.applyBtn));
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.rollRateKp), "RollRatePID", 0);
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.rollRateKi), "RollRatePID", 1);
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.pitchRateKp), "PitchRatePID", 0);
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.pitchRateKi), "PitchRatePID", 1);
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.rollKp), "RollPI", 0);
smartSave.addControlMapping((ScrollBarView) findViewById(R.id.pitchKp), "PitchPI", 0);
smartSave.refreshSettingsDisplay();
}
}

2
androidgcs/src/org/openpilot/androidgcs/UAVLocation.java
View File

@ -169,9 +169,11 @@ public class UAVLocation extends MapActivity
void onOPConnected() {
UAVObject obj = objMngr.getObject("HomeLocation");
registerObjectUpdates(obj);
objectUpdated(obj);
obj = objMngr.getObject("PositionActual");
registerObjectUpdates(obj);
objectUpdated(obj);
}
private GeoPoint getUavLocation() {

109
androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java
View File

@ -28,10 +28,6 @@ import java.io.IOException;
import java.util.Set;
import java.util.UUID;
import org.openpilot.uavtalk.UAVObjectManager;
import org.openpilot.uavtalk.UAVTalk;
import android.annotation.TargetApi;
import android.app.Activity;
import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
@ -43,11 +39,13 @@ import android.content.SharedPreferences;
import android.preference.PreferenceManager;
import android.util.Log;
@TargetApi(10) public class BluetoothUAVTalk {
public class BluetoothUAVTalk extends TelemetryTask {
private final String TAG = "BluetoothUAVTalk";
public static int LOGLEVEL = 2;
public static boolean WARN = LOGLEVEL > 1;
public static boolean DEBUG = LOGLEVEL > 0;
public static final int LOGLEVEL = 4;
public static final boolean DEBUG = LOGLEVEL > 2;
public static final boolean WARN = LOGLEVEL > 1;
public static final boolean ERROR = LOGLEVEL > 0;
// Temporarily define fixed device name
private String device_name = "RN42-222D";
@ -56,30 +54,35 @@ import android.util.Log;
private BluetoothAdapter mBluetoothAdapter;
private BluetoothSocket socket;
private BluetoothDevice device;
private UAVTalk uavTalk;
private boolean connected;
public BluetoothUAVTalk(Context caller) {
public BluetoothUAVTalk(OPTelemetryService caller) {
super(caller);
}
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller);
@Override
boolean attemptConnection() {
if( getConnected() )
return true;
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(telemService);
device_name = prefs.getString("bluetooth_mac","");
if (DEBUG) Log.d(TAG, "Trying to open UAVTalk with " + device_name);
connected = false;
device = null;
mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
if (mBluetoothAdapter == null) {
// Device does not support Bluetooth
Log.e(TAG, "Device does not support Bluetooth");
return;
return false;
}
if (!mBluetoothAdapter.isEnabled()) {
// Enable bluetooth if it isn't already
Intent enableBtIntent = new Intent(BluetoothAdapter.ACTION_REQUEST_ENABLE);
caller.sendOrderedBroadcast(enableBtIntent, "android.permission.BLUETOOTH_ADMIN", new BroadcastReceiver() {
telemService.sendOrderedBroadcast(enableBtIntent, "android.permission.BLUETOOTH_ADMIN", new BroadcastReceiver() {
@Override
public void onReceive(Context context, Intent intent) {
Log.e(TAG,"Received " + context + intent);
@ -90,60 +93,60 @@ import android.util.Log;
} else {
queryDevices();
}
}
public boolean connect(UAVObjectManager objMngr) {
if( getConnected() )
return true;
if( !getFoundDevice() )
return false;
if( !openTelemetryBluetooth(objMngr) )
return false;
return true;
}
public boolean getConnected() {
return connected;
@Override
public void disconnect() {
super.disconnect();
if(socket != null) {
try {
socket.close();
} catch (IOException e) {
if (ERROR) Log.e(TAG, "Unable to close BT socket");
}
socket = null;
}
}
public boolean getFoundDevice() {
return (device != null);
}
public UAVTalk getUavtalk() {
return uavTalk;
}
private void queryDevices() {
Log.d(TAG, "Searching for devices");
if (DEBUG) Log.d(TAG, "Searching for devices matching the selected preference");
Set<BluetoothDevice> pairedDevices = mBluetoothAdapter.getBondedDevices();
// If there are paired devices
if (pairedDevices.size() > 0) {
// Loop through paired devices
for (BluetoothDevice device : pairedDevices) {
// Add the name and address to an array adapter to show in a ListView
//mArrayAdapter.add(device.getName() + "\n" + device.getAddress());
Log.d(TAG, "Paired device: " + device.getAddress() + " compared to " + device_name);
if(device.getAddress().compareTo(device_name) == 0) {
Log.d(TAG, "Found device: " + device.getName());
if (DEBUG) Log.d(TAG, "Found selected device: " + device.getName());
this.device = device;
openTelemetryBluetooth();
return;
}
}
}
attemptedFailed();
}
private boolean openTelemetryBluetooth(UAVObjectManager objMngr) {
Log.d(TAG, "Opening connection to " + device.getName());
private boolean openTelemetryBluetooth() {
if (DEBUG) Log.d(TAG, "Opening connection to " + device.getName());
socket = null;
connected = false;
try {
socket = device.createInsecureRfcommSocketToServiceRecord(MY_UUID);
} catch (IOException e) {
Log.e(TAG,"Unable to create Rfcomm socket");
if (ERROR) Log.e(TAG,"Unable to create Rfcomm socket");
return false;
//e.printStackTrace();
}
mBluetoothAdapter.cancelDiscovery();
@ -152,26 +155,40 @@ import android.util.Log;
socket.connect();
}
catch (IOException e) {
Log.e(TAG,"Unable to connect to requested device", e);
if (ERROR) Log.e(TAG,"Unable to connect to requested device", e);
try {
socket.close();
} catch (IOException e2) {
Log.e(TAG, "unable to close() socket during connection failure", e2);
if (ERROR) Log.e(TAG, "unable to close() socket during connection failure", e2);
}
attemptedFailed();
return false;
}
connected = true;
try {
uavTalk = new UAVTalk(socket.getInputStream(), socket.getOutputStream(), objMngr);
inStream = socket.getInputStream();
outStream = socket.getOutputStream();
} catch (IOException e) {
Log.e(TAG,"Error starting UAVTalk");
// TODO Auto-generated catch block
//e.printStackTrace();
try {
socket.close();
} catch (IOException e2) {
}
attemptedFailed();
return false;
}
telemService.toastMessage("Bluetooth device connected");
// Post message to call attempt succeeded on the parent class
handler.post(new Runnable() {
@Override
public void run() {
BluetoothUAVTalk.this.attemptSucceeded();
}
});
return true;
}

5
androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java
View File

@ -121,7 +121,8 @@ public class OPTelemetryService extends Service {
break;
case 2:
Toast.makeText(getApplicationContext(), "Attempting BT connection", Toast.LENGTH_SHORT).show();
//activeTelem = new BTTelemetryThread();
telemTask = new BluetoothUAVTalk(this);
activeTelem = new Thread(telemTask, "Bluetooth telemetry thread");
break;
case 3:
Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show();
@ -226,7 +227,7 @@ public class OPTelemetryService extends Service {
public void onDestroy() {
if (telemTask != null) {
Log.d(TAG, "onDestory() shutting down telemetry task");
Log.d(TAG, "onDestroy() shutting down telemetry task");
telemTask.disconnect();
telemTask = null;

14
androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java
View File

@ -68,6 +68,9 @@ public abstract class TelemetryTask implements Runnable {
//! The telemetry monitor which takes care of high level connects / disconnects
private TelemetryMonitor mon;
//! Thread to process the input stream
Thread inputProcessThread;
//! Flag to indicate a shut down was requested. Derived classes should take care to respect this.
boolean shutdown;
@ -146,6 +149,14 @@ public abstract class TelemetryTask implements Runnable {
}
});
if (inputProcessThread != null) {
inputProcessThread.interrupt();
try {
inputProcessThread.join();
} catch (InterruptedException e) {
}
}
// TODO: Make sure the input and output stream is closed
// TODO: Make sure any threads for input and output are closed
@ -157,7 +168,8 @@ public abstract class TelemetryTask implements Runnable {
* to read from the input stream.
*/
private void startInputProcessing() {
new Thread(new processUavTalk(), "Process UAV talk").start();
inputProcessThread = new Thread(new processUavTalk(), "Process UAV talk");
inputProcessThread.start();
}
//! Runnable to process input stream

6
androidgcs/src/org/openpilot/androidgcs/util/ObjectFieldMappable.java Normal file
View File

@ -0,0 +1,6 @@
package org.openpilot.androidgcs.util;
public interface ObjectFieldMappable {
public double getValue();
public void setValue(double val);
}

256
androidgcs/src/org/openpilot/androidgcs/util/SmartSave.java Normal file
View File

@ -0,0 +1,256 @@
/**
******************************************************************************
* @file SmartSave.java
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Provides a handler to provide robust apply and save for settings
* and updating of the UI when the object changes.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
package org.openpilot.androidgcs.util;
import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;
import java.util.Observable;
import java.util.Observer;
import java.util.Set;
import junit.framework.Assert;
import org.openpilot.uavtalk.UAVObject;
import org.openpilot.uavtalk.UAVObjectManager;
import android.util.Log;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;
public class SmartSave {
private final static String TAG = SmartSave.class.getSimpleName();
private final static boolean DEBUG = false;
//! Create a smart save button attached to the object manager and an apply and ave button
public SmartSave(UAVObjectManager objMngr, UAVObject obj, Button saveButton, Button applyButton) {
Assert.assertNotNull(objMngr);
this.objMngr = objMngr;
this.applyBtn = applyButton;
this.obj = obj;
Assert.assertNotNull(objMngr);
Assert.assertNotNull(obj);
obj.addUpdatedObserver(ObjectUpdated);
controlFieldMapping = new HashMap<ObjectFieldMappable,FieldPairing>();
if (saveButton != null) {
saveBtn = saveButton;
saveBtn.setOnClickListener(new OnClickListener() {
@Override
public void onClick(View v) {
saveSettings();
}
});
} else
saveBtn = null;
if (applyButton != null) {
applyBtn = applyButton;
applyBtn.setOnClickListener(new OnClickListener() {
@Override
public void onClick(View v) {
applySettings();
}
});
} else
applyBtn = null;
}
//! Disconnect any listeners when this object is destroyed
public void disconnect() {
obj.removeUpdatedObserver(ObjectUpdated);
}
/**
* Add a control to this SmartSave object which maps between a particular control
* and a UAVO field.
* @param control The control to associate with
* @param fieldName The name of the UAVO field
* @param fieldIndex The index of the UAVO field
*/
public void addControlMapping(ObjectFieldMappable control, String fieldName, int fieldIndex) {
FieldPairing pairing = new FieldPairing(fieldName, fieldIndex);
controlFieldMapping.put(control, pairing);
}
//! Update the settings in the UI from the mappings
public void refreshSettingsDisplay() {
if (DEBUG) Log.d(TAG, "Refreshing display");
Set<ObjectFieldMappable> keys = controlFieldMapping.keySet();
Iterator<ObjectFieldMappable> iter = keys.iterator();
while(iter.hasNext()) {
ObjectFieldMappable mappable = iter.next();
FieldPairing field = controlFieldMapping.get(mappable);
mappable.setValue(field.getValue(obj));
}
}
/**
* Call applySettings() and then save them
* @return True if the save succeeded or false otherwise
*/
private boolean saveSettings() {
/*
* 1. Update object
* 2. Install listener on object persistence
* 3. Send save operation
* 4. Wait for completed
* 5. Remove listener
*/
// 1. Update object
if(!applySettings())
return false;
UAVObject persistence = objMngr.getObject("ObjectPersistence");
Assert.assertNotNull(persistence);
// 2. Install listener
persistence.addUpdatedObserver(ObjectPersistenceUpdated);
// 3. Send save operation
Long objId = obj.getObjID();
if (DEBUG) Log.d(TAG, "Saving object ID: " + objId);
persistence.getField("ObjectID").setValue(objId);
persistence.getField("Operation").setValue("Save");
persistence.getField("Selection").setValue("SingleObject");
persistence.updated();
return true;
}
/**
* Robustly apply the settings to the UAV
* @return True if the apply is ack'd, False if not
*/
private boolean applySettings() {
/*
* 1. Set the values from the fields into the object
* 2. Install the listener on the object
* 3. Update object
* 4. Wait for the acknowledgment or timeout
* 5. Uninstall the listener
*/
// 1. Set the fields in the object from the UI
Set<ObjectFieldMappable> keys = controlFieldMapping.keySet();
Iterator<ObjectFieldMappable> iter = keys.iterator();
while(iter.hasNext()) {
ObjectFieldMappable mappable = iter.next();
FieldPairing field = controlFieldMapping.get(mappable);
field.setValue(obj,mappable.getValue());
}
// 2. Install the listener on the object
obj.addTransactionCompleted(ApplyCompleted);
// 3. Update the object
obj.updated();
// 4. Wait for acknowledgment
// TODO: Set up some semaphore with timeout
// sem.wait(1000);
// 5. Uninstall the listener
//obj.removeTransactionCompleted(ApplyCompleted);
return true;
}
//! Private class to store the field mapping information
private class FieldPairing {
FieldPairing(String fieldName, int fieldIndex) {
this.fieldName = fieldName;
this.fieldIndex = fieldIndex;
}
//! Update the field in the UAVO
void setValue(UAVObject obj, double value) {
Assert.assertNotNull(obj);
obj.getField(fieldName).setDouble(value,fieldIndex);
}
//! Get the value from the UAVO field
double getValue(UAVObject obj) {
double val = obj.getField(fieldName).getDouble(fieldIndex);
if (DEBUG) Log.d(TAG, "Getting value from: " + fieldName + " " + val);
return obj.getField(fieldName).getDouble(fieldIndex);
}
//! Cache the name of the field
private final String fieldName;
//! Cache the field index
private final int fieldIndex;
}
//! Installed on monitored object to know when an object is updated
private final Observer ApplyCompleted = new Observer() {
@Override
public void update(Observable observable, Object data) {
if (DEBUG) Log.d(TAG, "Apply called");
}
};
//! Installed on monitored object to know when an object is updated
private final Observer ObjectUpdated = new Observer() {
@Override
public void update(Observable observable, Object data) {
if (DEBUG) Log.d(TAG, "Object updated");
refreshSettingsDisplay();
}
};
//! Installed on object persistence to know when save completes
private final Observer ObjectPersistenceUpdated = new Observer() {
@Override
public void update(Observable observable, Object data) {
if (DEBUG) Log.d(TAG, "Object persistence updated");
}
};
//! Map of all the UAVO field <-> control mappings. The key is the control.
private final Map<ObjectFieldMappable,FieldPairing> controlFieldMapping;
//! Handle to the object manager
private final UAVObjectManager objMngr;
//! Handle to the apply button
private Button applyBtn;
//! Handle to the save button
private Button saveBtn;
//! Handle to the UAVO this class works with
private final UAVObject obj;
}

129
androidgcs/src/org/openpilot/androidgcs/views/ScrollBarView.java Normal file
View File

@ -0,0 +1,129 @@
package org.openpilot.androidgcs.views;
import org.openpilot.androidgcs.R;
import org.openpilot.androidgcs.util.ObjectFieldMappable;
import android.content.Context;
import android.content.res.TypedArray;
import android.text.Editable;
import android.text.TextWatcher;
import android.util.AttributeSet;
import android.util.Log;
import android.widget.EditText;
import android.widget.GridLayout;
import android.widget.LinearLayout;
import android.widget.SeekBar;
import android.widget.SeekBar.OnSeekBarChangeListener;
import android.widget.TextView;
public class ScrollBarView extends GridLayout implements ObjectFieldMappable {
private final static String TAG = ScrollBarView.class.getSimpleName();
private final TextView lbl;
private final EditText edit;
private final SeekBar bar;
private double value;
private String name;
private final double SCALE = 1000000;
public ScrollBarView(Context context, AttributeSet attrs) {
super(context, attrs);
Log.d(TAG, "Scroll bar init called");
setOrientation(LinearLayout.VERTICAL);
setColumnCount(2);
lbl = new TextView(context);
TypedArray ta = context.obtainStyledAttributes(attrs, R.styleable.setting_attributes, 0, 0);
lbl.setText(ta.getString(R.styleable.setting_attributes_setting_name));
addView(lbl, new GridLayout.LayoutParams(spec(0), spec(0)));
edit = new EditText(context);
addView(edit, new GridLayout.LayoutParams(spec(0), spec(1)));
bar = new SeekBar(context);
addView(bar, new GridLayout.LayoutParams(spec(1), spec(0,2)));
ta = context.obtainStyledAttributes(attrs, R.styleable.setting_attributes, 0, 0);
final double max = ta.getFloat(R.styleable.setting_attributes_max_value,0);
bar.setMax((int) (SCALE * max));
// Update the value when the progress bar changes
bar.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {
@Override
public void onProgressChanged(SeekBar seekBar, int progress,
boolean fromUser) {
value = progress / SCALE;
edit.setText(Double.toString(value));
}
@Override
public void onStartTrackingTouch(SeekBar seekBar) {
}
@Override
public void onStopTrackingTouch(SeekBar seekBar) {
}
});
// Update the value when the edit box changes
edit.addTextChangedListener(new TextWatcher() {
@Override
public void afterTextChanged(Editable s) {
value = Double.parseDouble(s.toString());
bar.setProgress((int) (SCALE * value));
}
@Override
public void beforeTextChanged(CharSequence s, int start, int count,
int after) {
}
@Override
public void onTextChanged(CharSequence s, int start, int before,
int count) {
}
});
setPadding(5,5,5,5);
setMinimumWidth(300);
setValue(0.0035);
}
public void setName(String n)
{
name = n;
lbl.setText(name);
}
@Override
protected void onMeasure(int widthMeasureSpec, int heightMeasureSpec) {
super.onMeasure(widthMeasureSpec, heightMeasureSpec);
// This shouldn't be needed if I could make this scroll bar
// automagically span both columns
android.view.ViewGroup.LayoutParams param = bar.getLayoutParams();
param.width = (int) (getMeasuredWidth() * 0.9);
// Force the label to half the page width
param = lbl.getLayoutParams();
param.width = getMeasuredWidth() / 2;
}
@Override
public double getValue() {
return value;
}
@Override
public void setValue(double val) {
value = val;
edit.setText(Double.toString(value));
bar.setProgress((int) (SCALE * value));
}
}

4697
artwork/Android/hdpi/_pre_production/hdpi_icons.ai
View File

File diff suppressed because one or more lines are too long

4619
artwork/Android/hdpi/_pre_production/pfd.ai
View File

File diff suppressed because one or more lines are too long

BIN
artwork/Android/hdpi/ic_logging.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 823 B

After

Width:  |  Height:  |  Size: 849 B

BIN
artwork/Android/hdpi/ic_tuning.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.9 KiB

BIN
artwork/Android/hdpi/im_pfd_fixed.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.4 KiB

BIN
artwork/Android/hdpi/im_pfd_horizon.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.3 KiB

After

Width:  |  Height:  |  Size: 9.7 KiB

BIN
artwork/Android/hdpi/im_pfd_reticule.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

383
flight/Bootloaders/RevoMini/Makefile Normal file
View File

@ -0,0 +1,383 @@
#####
# Project: OpenPilot INS_BOOTLOADER
#
#
# Makefile for OpenPilot INS project
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program 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.
#
# This program 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 this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := bl_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
# Paths
REVO_BL = $(WHEREAMI)
REVO_BLINC = $(REVO_BL)/inc
PIOS = ../../PiOS
PIOSINC = $(PIOS)/inc
FLIGHTLIB = ../../Libraries
FLIGHTLIBINC = ../../Libraries/inc
PIOSSTM32F4XX = $(PIOS)/STM32F4xx
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
APPLIBDIR = $(PIOSSTM32F4XX)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F4xx_StdPeriph_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## BOOTLOADER:
SRC += main.c
SRC += pios_board.c
SRC += pios_usb_board_data.c
SRC += op_dfu.c
## PIOS Hardware (STM32F4xx)
include $(PIOS)/STM32F4xx/library.mk
# PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_board_info.c
SRC += $(PIOSCOMMON)/pios_com_msg.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
SRC += $(PIOSCOMMON)/pios_usb_util.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSSTM34FXX)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(REVO_BLINC)
EXTRAINCDIRS += $(HWDEFSINC)
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32FXX)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS = -DSTM32F4XX
CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
# Provide (only) the bootloader with board-specific defines
BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS =
endif
# This is not the best place for these. Really should abstract out
# to the board file or something
CFLAGS += -DSTM32F4XX
CFLAGS += -DMEM_SIZE=1024000000
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
ifeq ($(DEBUG),NO)
CFLAGS += -fdata-sections -ffunction-sections
endif
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(BLONLY_CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
#CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
ifeq ($(DEBUG),NO)
LDFLAGS += -Wl,-static
endif
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
#Linker scripts
LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_BL))
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: build
ifeq ($(LOADFORMAT),ihex)
build: elf hex sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).bin
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).bin
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

115
flight/Bootloaders/RevoMini/inc/common.h Normal file
View File

@ -0,0 +1,115 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
*
* @{
* @file common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains various common defines for the BootLoader
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef COMMON_H_
#define COMMON_H_
//#include "board.h"
typedef enum {
start, keepgoing,
} DownloadAction;
/**************************************************/
/* OP_DFU states */
/**************************************************/
typedef enum {
DFUidle, //0
uploading, //1
wrong_packet_received, //2
too_many_packets, //3
too_few_packets, //4
Last_operation_Success, //5
downloading, //6
BLidle, //7
Last_operation_failed, //8
uploadingStarting, //9
outsideDevCapabilities, //10
CRC_Fail,//11
failed_jump,
//12
} DFUStates;
/**************************************************/
/* OP_DFU commands */
/**************************************************/
typedef enum {
Reserved, //0
Req_Capabilities, //1
Rep_Capabilities, //2
EnterDFU, //3
JumpFW, //4
Reset, //5
Abort_Operation, //6
Upload, //7
Op_END, //8
Download_Req, //9
Download, //10
Status_Request, //11
Status_Rep
//12
} DFUCommands;
typedef enum {
High_Density, Medium_Density
} DeviceType;
/**************************************************/
/* OP_DFU transfer types */
/**************************************************/
typedef enum {
FW, //0
Descript
//2
} DFUTransfer;
/**************************************************/
/* OP_DFU transfer port */
/**************************************************/
typedef enum {
Usb, //0
Serial
//2
} DFUPort;
/**************************************************/
/* OP_DFU programable programable HW types */
/**************************************************/
typedef enum {
Self_flash, //0
Remote_flash_via_spi
//1
} DFUProgType;
/**************************************************/
/* OP_DFU programable sources */
/**************************************************/
#define USB 0
#define SPI 1
#define DownloadDelay 100000
#define MAX_DEL_RETRYS 3
#define MAX_WRI_RETRYS 3
#endif /* COMMON_H_ */

60
flight/Bootloaders/RevoMini/inc/op_dfu.h Normal file
View File

@ -0,0 +1,60 @@
/**
******************************************************************************
*
* @file op_dfu.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __OP_DFU_H
#define __OP_DFU_H
#include "common.h"
/* Includes ------------------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
typedef struct {
uint8_t programmingType;
uint8_t readWriteFlags;
uint32_t startOfUserCode;
uint32_t sizeOfCode;
uint8_t sizeOfDescription;
uint8_t BL_Version;
uint16_t devID;
DeviceType devType;
uint32_t FW_Crc;
} Device;
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported define -----------------------------------------------------------*/
#define COMMAND 0
#define COUNT 1
#define DATA 5
/* Exported functions ------------------------------------------------------- */
void processComand(uint8_t *Receive_Buffer);
uint32_t baseOfAdressType(uint8_t type);
uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size);
void OPDfuIni(uint8_t discover);
void DataDownload(DownloadAction);
bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type);
#endif /* __OP_DFU_H */
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/

43
flight/Bootloaders/RevoMini/inc/pios_config.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM_MSG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
#endif /* PIOS_CONFIG_H */

52
flight/Bootloaders/RevoMini/inc/pios_usb_board_data.h Normal file
View File

@ -0,0 +1,52 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2
#include "pios_usb_defs.h" /* struct usb_* */
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL)
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
/*
* The bootloader uses a simplified report structure
* BL: <REPORT_ID><DATA>...<DATA>
* FW: <REPORT_ID><LENGTH><DATA>...<DATA>
* This define changes the behaviour in pios_usb_hid.c
*/
#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
#endif /* PIOS_USB_BOARD_DATA_H */

230
flight/Bootloaders/RevoMini/main.c Normal file
View File

@ -0,0 +1,230 @@
/**
******************************************************************************
* @addtogroup RevolutionBL Revolution BootLoader
* @brief These files contain the code to the Revolution Bootloader.
*
* @{
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This is the file with the main function of the Revolution BootLoader
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Bootloader Includes */
#include <pios.h>
#include <pios_board_info.h>
#include <stdbool.h>
#include "op_dfu.h"
#include "pios_iap.h"
#include "fifo_buffer.h"
#include "pios_com_msg.h"
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
void check_bor();
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
/* Private typedef -----------------------------------------------------------*/
typedef void (*pFunction)(void);
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
////////////////////////////////////////
uint8_t tempcount = 0;
/* Extern variables ----------------------------------------------------------*/
DFUStates DeviceState;
int16_t status = 0;
bool JumpToApp = false;
bool GO_dfu = false;
bool USB_connected = false;
bool User_DFU_request = false;
static uint8_t mReceive_Buffer[63];
/* Private function prototypes -----------------------------------------------*/
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
uint8_t processRX();
void jump_to_app();
int main() {
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
// Make sure the brown out reset value for this chip
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = true;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == true) || (User_DFU_request == true);
if (GO_dfu == true) {
if (User_DFU_request == true)
DeviceState = DFUidle;
else
DeviceState = BLidle;
} else
JumpToApp = true;
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (true) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == true)
jump_to_app();
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default://error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_On(PIOS_LED_HEARTBEAT);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
if (stopwatch > 50 * 1000 * 1000)
stopwatch = 0;
if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
== BLidle))
JumpToApp = true;
processRX();
DataDownload(start);
}
}
void jump_to_app() {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
PIOS_USBHOOK_Deactivate();
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
Jump_To_Application = (pFunction) JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
Jump_To_Application();
} else {
DeviceState = failed_jump;
return;
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX() {
if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
processComand(mReceive_Buffer);
}
return true;
}
/**
* Check the brown out reset threshold is 2.7 volts and if not
* resets it. This solves an issue that can prevent boards
* powering up with some BEC
*/
void check_bor()
{
uint8_t bor = FLASH_OB_GetBOR();
if(bor != OB_BOR_LEVEL3) {
FLASH_OB_Unlock();
FLASH_OB_BORConfig(OB_BOR_LEVEL3);
FLASH_OB_Launch();
while(FLASH_WaitForLastOperation() == FLASH_BUSY);
FLASH_OB_Lock();
while(FLASH_WaitForLastOperation() == FLASH_BUSY);
}
}

468
flight/Bootloaders/RevoMini/op_dfu.c Normal file
View File

@ -0,0 +1,468 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
*
* @{
* @file op_dfu.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains the DFU commands handling code
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Includes ------------------------------------------------------------------*/
#include "pios.h"
#include <stdbool.h>
#include "op_dfu.h"
#include "pios_bl_helper.h"
#include "pios_com_msg.h"
#include <pios_board_info.h>
//programmable devices
Device devicesTable[10];
uint8_t numberOfDevices = 0;
DFUProgType currentProgrammingDestination; //flash, flash_trough spi
uint8_t currentDeviceCanRead;
uint8_t currentDeviceCanWrite;
Device currentDevice;
uint8_t Buffer[64];
uint8_t echoBuffer[64];
uint8_t SendBuffer[64];
uint8_t Command = 0;
uint8_t EchoReqFlag = 0;
uint8_t EchoAnsFlag = 0;
uint8_t StartFlag = 0;
uint32_t Aditionals = 0;
uint32_t SizeOfTransfer = 0;
uint32_t Expected_CRC = 0;
uint8_t SizeOfLastPacket = 0;
uint32_t Next_Packet = 0;
uint8_t TransferType;
uint32_t Count = 0;
uint32_t Data;
uint8_t Data0;
uint8_t Data1;
uint8_t Data2;
uint8_t Data3;
uint8_t offset = 0;
uint32_t aux;
//Download vars
uint32_t downSizeOfLastPacket = 0;
uint32_t downPacketTotal = 0;
uint32_t downPacketCurrent = 0;
DFUTransfer downType = 0;
/* Extern variables ----------------------------------------------------------*/
extern DFUStates DeviceState;
extern uint8_t JumpToApp;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
void sendData(uint8_t * buf, uint16_t size);
uint32_t CalcFirmCRC(void);
void DataDownload(DownloadAction action) {
if ((DeviceState == downloading)) {
uint8_t packetSize;
uint32_t offset;
uint32_t partoffset;
SendBuffer[0] = 0x01;
SendBuffer[1] = Download;
SendBuffer[2] = downPacketCurrent >> 24;
SendBuffer[3] = downPacketCurrent >> 16;
SendBuffer[4] = downPacketCurrent >> 8;
SendBuffer[5] = downPacketCurrent;
if (downPacketCurrent == downPacketTotal - 1) {
packetSize = downSizeOfLastPacket;
} else {
packetSize = 14;
}
for (uint8_t x = 0; x < packetSize; ++x) {
partoffset = (downPacketCurrent * 14 * 4) + (x * 4);
offset = baseOfAdressType(downType) + partoffset;
if (!flash_read(SendBuffer + (6 + x * 4), offset,
currentProgrammingDestination)) {
DeviceState = Last_operation_failed;
}
}
downPacketCurrent = downPacketCurrent + 1;
if (downPacketCurrent > downPacketTotal - 1) {
DeviceState = Last_operation_Success;
Aditionals = (uint32_t) Download;
}
sendData(SendBuffer + 1, 63);
}
}
void processComand(uint8_t *xReceive_Buffer) {
Command = xReceive_Buffer[COMMAND];
#ifdef DEBUG_SSP
char str[63]= {0};
sprintf(str,"Received COMMAND:%d|",Command);
PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
#endif
EchoReqFlag = (Command >> 7);
EchoAnsFlag = (Command >> 6) & 0x01;
StartFlag = (Command >> 5) & 0x01;
Count = xReceive_Buffer[COUNT] << 24;
Count += xReceive_Buffer[COUNT + 1] << 16;
Count += xReceive_Buffer[COUNT + 2] << 8;
Count += xReceive_Buffer[COUNT + 3];
Data = xReceive_Buffer[DATA] << 24;
Data += xReceive_Buffer[DATA + 1] << 16;
Data += xReceive_Buffer[DATA + 2] << 8;
Data += xReceive_Buffer[DATA + 3];
Data0 = xReceive_Buffer[DATA];
Data1 = xReceive_Buffer[DATA + 1];
Data2 = xReceive_Buffer[DATA + 2];
Data3 = xReceive_Buffer[DATA + 3];
Command = Command & 0b00011111;
if (EchoReqFlag == 1) {
memcpy(echoBuffer, xReceive_Buffer, 64);
}
switch (Command) {
case EnterDFU:
if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
|| (DeviceState == DFUidle)) {
if (Data0 > 0)
OPDfuIni(true);
DeviceState = DFUidle;
currentProgrammingDestination = devicesTable[Data0].programmingType;
currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01;
currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1
& 0x01;
currentDevice = devicesTable[Data0];
uint8_t result = 0;
switch (currentProgrammingDestination) {
case Self_flash:
result = PIOS_BL_HELPER_FLASH_Ini();
break;
case Remote_flash_via_spi:
result = true;
break;
default:
DeviceState = Last_operation_failed;
Aditionals = (uint16_t) Command;
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
}
break;
case Upload:
if ((DeviceState == DFUidle) || (DeviceState == uploading)) {
if ((StartFlag == 1) && (Next_Packet == 0)) {
TransferType = Data0;
SizeOfTransfer = Count;
Next_Packet = 1;
Expected_CRC = Data2 << 24;
Expected_CRC += Data3 << 16;
Expected_CRC += xReceive_Buffer[DATA + 4] << 8;
Expected_CRC += xReceive_Buffer[DATA + 5];
SizeOfLastPacket = Data1;
if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
* 14 * 4 + SizeOfLastPacket * 4) == true) {
DeviceState = outsideDevCapabilities;
Aditionals = (uint32_t) Command;
} else {
uint8_t result = 1;
if (TransferType == FW) {
switch (currentProgrammingDestination) {
case Self_flash:
result = PIOS_BL_HELPER_FLASH_Start();
break;
case Remote_flash_via_spi:
result = false;
break;
default:
break;
}
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
} else {
DeviceState = uploading;
}
}
} else if ((StartFlag != 1) && (Next_Packet != 0)) {
if (Count > SizeOfTransfer) {
DeviceState = too_many_packets;
Aditionals = Count;
} else if (Count == Next_Packet - 1) {
uint8_t numberOfWords = 14;
if (Count == SizeOfTransfer - 1)//is this the last packet?
{
numberOfWords = SizeOfLastPacket;
}
uint8_t result = 0;
switch (currentProgrammingDestination) {
case Self_flash:
for (uint8_t x = 0; x < numberOfWords; ++x) {
offset = 4 * x;
Data = xReceive_Buffer[DATA + offset] << 24;
Data += xReceive_Buffer[DATA + 1 + offset] << 16;
Data += xReceive_Buffer[DATA + 2 + offset] << 8;
Data += xReceive_Buffer[DATA + 3 + offset];
aux = baseOfAdressType(TransferType) + (uint32_t)(
Count * 14 * 4 + x * 4);
result = 0;
for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
if (result == 0) {
result = (FLASH_ProgramWord(aux, Data)
== FLASH_COMPLETE) ? 1 : 0;
}
}
}
break;
case Remote_flash_via_spi:
result = false; // No support for this for the PipX
break;
default:
result = 0;
break;
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
++Next_Packet;
} else {
DeviceState = wrong_packet_received;
Aditionals = Count;
}
} else {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
}
break;
case Req_Capabilities:
OPDfuIni(true);
Buffer[0] = 0x01;
Buffer[1] = Rep_Capabilities;
if (Data0 == 0) {
Buffer[2] = 0;
Buffer[3] = 0;
Buffer[4] = 0;
Buffer[5] = 0;
Buffer[6] = 0;
Buffer[7] = numberOfDevices;
uint16_t WRFlags = 0;
for (int x = 0; x < numberOfDevices; ++x) {
WRFlags = ((devicesTable[x].readWriteFlags << (x * 2))
| WRFlags);
}
Buffer[8] = WRFlags >> 8;
Buffer[9] = WRFlags;
} else {
Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24;
Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16;
Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8;
Buffer[5] = devicesTable[Data0 - 1].sizeOfCode;
Buffer[6] = Data0;
Buffer[7] = devicesTable[Data0 - 1].BL_Version;
Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription;
Buffer[9] = devicesTable[Data0 - 1].devID;
Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24;
Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16;
Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8;
Buffer[13] = devicesTable[Data0 - 1].FW_Crc;
Buffer[14] = devicesTable[Data0 - 1].devID >> 8;
Buffer[15] = devicesTable[Data0 - 1].devID;
}
sendData(Buffer + 1, 63);
break;
case JumpFW:
if (Data == 0x5AFE) {
/* Force board into safe mode */
PIOS_IAP_WriteBootCount(0xFFFF);
}
FLASH_Lock();
JumpToApp = 1;
break;
case Reset:
PIOS_SYS_Reset();
break;
case Abort_Operation:
Next_Packet = 0;
DeviceState = DFUidle;
break;
case Op_END:
if (DeviceState == uploading) {
if (Next_Packet - 1 == SizeOfTransfer) {
Next_Packet = 0;
if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) {
DeviceState = Last_operation_Success;
} else {
DeviceState = CRC_Fail;
}
}
if (Next_Packet - 1 < SizeOfTransfer) {
Next_Packet = 0;
DeviceState = too_few_packets;
}
}
break;
case Download_Req:
#ifdef DEBUG_SSP
sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState);
PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
#endif
if (DeviceState == DFUidle) {
#ifdef DEBUG_SSP
PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|");
#endif
downType = Data0;
downPacketTotal = Count;
downSizeOfLastPacket = Data1;
if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 * 4
+ downSizeOfLastPacket * 4) == 1) {
DeviceState = outsideDevCapabilities;
Aditionals = (uint32_t) Command;
} else {
downPacketCurrent = 0;
DeviceState = downloading;
}
} else {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
break;
case Status_Request:
Buffer[0] = 0x01;
Buffer[1] = Status_Rep;
if (DeviceState == wrong_packet_received) {
Buffer[2] = Aditionals >> 24;
Buffer[3] = Aditionals >> 16;
Buffer[4] = Aditionals >> 8;
Buffer[5] = Aditionals;
} else {
Buffer[2] = 0;
Buffer[3] = ((uint16_t) Aditionals) >> 8;
Buffer[4] = ((uint16_t) Aditionals);
Buffer[5] = 0;
}
Buffer[6] = DeviceState;
Buffer[7] = 0;
Buffer[8] = 0;
Buffer[9] = 0;
sendData(Buffer + 1, 63);
if (DeviceState == Last_operation_Success) {
DeviceState = DFUidle;
}
break;
case Status_Rep:
break;
}
if (EchoReqFlag == 1) {
echoBuffer[0] = echoBuffer[0] | (1 << 6);
sendData(echoBuffer, 63);
}
return;
}
void OPDfuIni(uint8_t discover) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
Device dev;
dev.programmingType = Self_flash;
dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1));
dev.startOfUserCode = bdinfo->fw_base;
dev.sizeOfCode = bdinfo->fw_size;
dev.sizeOfDescription = bdinfo->desc_size;
dev.BL_Version = bdinfo->bl_rev;
dev.FW_Crc = CalcFirmCRC();
dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev);
dev.devType = bdinfo->hw_type;
numberOfDevices = 1;
devicesTable[0] = dev;
if (discover) {
//TODO check other devices trough spi or whatever
}
}
uint32_t baseOfAdressType(DFUTransfer type) {
switch (type) {
case FW:
return currentDevice.startOfUserCode;
break;
case Descript:
return currentDevice.startOfUserCode + currentDevice.sizeOfCode;
break;
default:
return 0;
}
}
uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) {
switch (type) {
case FW:
return (size > currentDevice.sizeOfCode) ? 1 : 0;
break;
case Descript:
return (size > currentDevice.sizeOfDescription) ? 1 : 0;
break;
default:
return true;
}
}
uint32_t CalcFirmCRC() {
switch (currentProgrammingDestination) {
case Self_flash:
return PIOS_BL_HELPER_CRC_Memory_Calc();
break;
case Remote_flash_via_spi:
return 0;
break;
default:
return 0;
break;
}
}
void sendData(uint8_t * buf, uint16_t size) {
PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
}
bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) {
switch (type) {
case Remote_flash_via_spi:
return false; // We should not get this for the PipX
break;
case Self_flash:
for (uint8_t x = 0; x < 4; ++x) {
buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
}
return true;
break;
default:
return false;
}
}

78
flight/Bootloaders/RevoMini/pios_board.c Normal file
View File

@ -0,0 +1,78 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
* the HW definitions to be const and static to limit their
* scope.
*
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/
#include "board_hw_defs.c"
#include <pios_board_info.h>
#include <pios.h>
uint32_t pios_com_telem_usb_id;
static bool board_init_complete = false;
void PIOS_Board_Init() {
if (board_init_complete) {
return;
}
/* Delay system */
PIOS_DELAY_Init();
#if defined(PIOS_INCLUDE_LED)
PIOS_LED_Init(&pios_led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Activate the HID-only USB configuration */
PIOS_USB_DESC_HID_ONLY_Init();
uint32_t pios_usb_id;
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
PIOS_USBHOOK_Activate();
#endif /* PIOS_INCLUDE_USB */
board_init_complete = true;
}

98
flight/Bootloaders/RevoMini/pios_usb_board_data.c Normal file
View File

@ -0,0 +1,98 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
#include "pios_usb_util.h" /* PIOS_USB_UTIL_AsciiToUtf8 */
static const uint8_t usb_product_id[22] = {
sizeof(usb_product_id),
USB_DESC_TYPE_STRING,
'R', 0,
'e', 0,
'v', 0,
'o', 0,
'l', 0,
'u', 0,
't', 0,
'i', 0,
'o', 0,
'n', 0,
};
static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
};
static const uint8_t usb_vendor_id[28] = {
sizeof(usb_vendor_id),
USB_DESC_TYPE_STRING,
'o', 0,
'p', 0,
'e', 0,
'n', 0,
'p', 0,
'i', 0,
'l', 0,
'o', 0,
't', 0,
'.', 0,
'o', 0,
'r', 0,
'g', 0
};
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
PIOS_SYS_SerialNumberGet((char *)sn);
/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
uint8_t * utf8 = &(usb_serial_number[2]);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
return 0;
}

11
flight/CopterControl/Makefile
View File

@ -68,6 +68,7 @@ USE_GPS ?= YES
USE_TXPID ?= YES
USE_I2C ?= NO
USE_ALTITUDE ?= NO
USE_AUTOTUNE ?= YES
TEST_FAULTS ?= NO
# List of optional modules to include
@ -94,6 +95,9 @@ endif
ifeq ($(TEST_FAULTS), YES)
OPTMODULES += Fault
endif
ifeq ($(USE_AUTOTUNE), YES)
OPTMODULES += Autotune
endif
# List of mandatory modules to include
MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
@ -111,6 +115,8 @@ OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = $(FLIGHTLIB)/inc
MATHLIB = ../Libraries/math
MATHLIBINC = ../Libraries/math
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
@ -204,6 +210,8 @@ SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
SRC += $(OPUAVSYNTHDIR)/relaytuning.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
@ -264,6 +272,8 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
@ -364,6 +374,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)

3
flight/CopterControl/System/pios_board.c
View File

@ -114,7 +114,8 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
};
#endif /* PIOS_INCLUDE_MPU6000 */

17
flight/EntireFlash/Makefile
View File

@ -33,16 +33,23 @@ OUTDIR := $(TOP)/build/$(TARGET)
.PHONY: bin
bin: $(OUTDIR)/$(TARGET).bin
FW_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)])
$(OUTDIR)/$(TARGET).pad:
FW_PRE_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)])
$(OUTDIR)/$(TARGET).fw_pre.pad:
$(V0) @echo $(MSG_PADDING) $@
$(V1) dd status=noxfer if=/dev/zero count=$(FW_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
$(V1) dd status=noxfer if=/dev/zero count=$(FW_PRE_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
FW_POST_PAD = $(shell echo $$[$(FW_BANK_SIZE)-$(FW_DESC_SIZE)-$$(stat -c%s $(FW_BIN))])
$(OUTDIR)/$(TARGET).fw_post.pad:
$(V0) @echo $(MSG_PADDING) $@
$(V1) dd status=noxfer if=/dev/zero count=$(FW_POST_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
BL_BIN = $(TOP)/build/bl_$(BOARD_NAME)/bl_$(BOARD_NAME).bin
FW_BIN = $(TOP)/build/fw_$(BOARD_NAME)/fw_$(BOARD_NAME).bin
$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).pad
FWINFO_BIN = $(FW_BIN).firmwareinfo.bin
$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).fw_pre.pad $(OUTDIR)/$(TARGET).fw_post.pad
$(V0) @echo $(MSG_FLASH_IMG) $@
$(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).pad $(FW_BIN) > $@
$(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).fw_pre.pad $(FW_BIN) $(OUTDIR)/$(TARGET).fw_post.pad $(FWINFO_BIN) > $@
.PHONY: dfu
dfu: $(OUTDIR)/$(TARGET).bin

2
flight/Libraries/aes.c
View File

@ -1,7 +1,7 @@
// http://gladman.plushost.co.uk/oldsite/AES/index.php
//#include <stdio.h>
#include <stdint.h>
#include "aes.h"

2
flight/Libraries/inc/aes.h
View File

@ -2,8 +2,6 @@
#ifndef _AES_H_
#define _AES_H_
#include "stm32f10x.h"
#define N_ROW 4
#define N_COL 4
#define N_BLOCK (N_ROW * N_COL)

12
flight/Libraries/inc/packet_handler.h
View File

@ -30,6 +30,9 @@
#ifndef __PACKET_HANDLER_H__
#define __PACKET_HANDLER_H__
#include <uavobjectmanager.h>
#include <gcsreceiver.h>
// Public defines / macros
#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p)
#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p)
@ -71,7 +74,7 @@ typedef struct {
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
uint16_t channels[GCSRECEIVER_CHANNEL_NUMELEM];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
@ -87,8 +90,10 @@ typedef struct {
} PHStatusPacket, *PHStatusPacketHandle;
typedef struct {
uint8_t winSize;
uint16_t maxConnections;
uint32_t default_destination_id;
uint32_t source_id;
uint16_t max_connections;
uint8_t win_size;
} PacketHandlerConfig;
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
@ -110,6 +115,7 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len);
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);

158
flight/Libraries/math/pid.c Normal file
View File

@ -0,0 +1,158 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "pid.h"
#define F_PI ((float) M_PI)
//! Private method
static float bound(float val, float range);
//! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
//! Store the setpoint weight to apply for the derivative term
static float deriv_gamma = 1.0;
/**
* Update the PID computation
* @param[in] pid The PID struture which stores temporary information
* @param[in] err The error term
* @param[in] dT The time step
* @returns Output the computed controller value
*/
float pid_apply(struct pid *pid, const float err, float dT)
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err;
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
}
/**
* Update the PID computation with setpoint weighting on the derivative
* @param[in] pid The PID struture which stores temporary information
* @param[in] setpoint The setpoint to use
* @param[in] measured The measured value of output
* @param[in] dT The time step
* @returns Output the computed controller value
*
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT)
{
float err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term,
float dterm = 0;
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
}
/**
* Reset a bit
* @param[in] pid The pid to reset
*/
void pid_zero(struct pid *pid)
{
if (!pid)
return;
pid->iAccumulator = 0;
pid->lastErr = 0;
pid->lastDer = 0;
}
/**
* @brief Configure the common terms that alter ther derivative
* @param[in] cutoff The cutoff frequency (in Hz)
* @param[in] gamma The gamma term for setpoint shaping (unsused now)
*/
void pid_configure_derivative(float cutoff, float g)
{
deriv_tau = 1.0f / (2 * F_PI * cutoff);
deriv_gamma = g;
}
/**
* Configure the settings for a pid structure
* @param[out] pid The PID structure to configure
* @param[in] p The proportional term
* @param[in] i The integral term
* @param[in] d The derivative term
*/
void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
{
if (!pid)
return;
pid->p = p;
pid->i = i;
pid->d = d;
pid->iLim = iLim;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}

52
flight/Libraries/math/pid.h Normal file
View File

@ -0,0 +1,52 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PID_H
#define PID_H
//!
struct pid {
float p;
float i;
float d;
float iLim;
float iAccumulator;
float lastErr;
float lastDer;
};
//! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);
#endif /* PID_H */

142
flight/Libraries/math/sin_lookup.c Normal file
View File

@ -0,0 +1,142 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file sin_lookup.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Sine lookup table from flash with 1 degree resolution
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "math.h"
#include "stdbool.h"
#include "stdint.h"
#define FLASH_TABLE
#ifdef FLASH_TABLE
/** Version of the code which precomputes the lookup table in flash **/
// This is a precomputed sin lookup table over 90 degrees that
const float sin_table[] = {
0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f,
0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f,
0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f,
0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f,
0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f,
0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f,
0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f,
0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f,
0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f,
1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f,
0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f,
0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f,
0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f,
0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f,
0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f,
0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f,
0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f,
0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f
};
int sin_lookup_initalize()
{
return 0;
}
#else
/** Version of the code which allocates the lookup table in heap **/
const int SIN_RESOLUTION = 180;
static float *sin_table;
int sin_lookup_initalize()
{
// Previously initialized
if (sin_table)
return 0;
sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION);
if (sin_table == NULL)
return -1;
for(uint32_t i = 0; i < 180; i++)
sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f);
return 0;
}
#endif
/**
* Use the lookup table to return sine(angle) where angle is in radians
* to save flash this cheats and uses trig functions to only save
* 90 values
* @param[in] angle Angle in degrees
* @returns sin(angle)
*/
float sin_lookup_deg(float angle)
{
if (sin_table == NULL)
return 0;
int i_ang = ((int32_t) angle) % 360;
if (i_ang >= 180) // for 180 to 360 deg
return -sin_table[i_ang - 180];
else // for 0 to 179 deg
return sin_table[i_ang];
return 0;
}
/**
* Get cos(angle) using the sine lookup table
* @param[in] angle Angle in degrees
* @returns cos(angle)
*/
float cos_lookup_deg(float angle)
{
return sin_lookup_deg(angle + 90);
}
/**
* Use the lookup table to return sine(angle) where angle is in radians
* @param[in] angle Angle in radians
* @returns sin(angle)
*/
float sin_lookup_rad(float angle)
{
int degrees = angle * 180.0f / M_PI;
return sin_lookup_deg(degrees);
}
/**
* Use the lookup table to return sine(angle) where angle is in radians
* @param[in] angle Angle in radians
* @returns cos(angle)
*/
float cos_lookup_rad(float angle)
{
int degrees = angle * 180.0f / M_PI;
return cos_lookup_deg(degrees);
}

40
flight/Libraries/math/sin_lookup.h Normal file
View File

@ -0,0 +1,40 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file sin_lookup.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Sine lookup table from flash with 1 degree resolution
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef SIN_LOOKUP_H
#define SIN_LOOKUP_H
int sin_lookup_initalize();
float sin_lookup_deg(float angle);
float cos_lookup_deg(float angle);
float sin_lookup_rad(float angle);
float cos_lookup_rad(float angle);
#endif

70
flight/Libraries/packet_handler.c
View File

@ -44,7 +44,6 @@ typedef struct {
uint8_t rx_win_start;
uint8_t rx_win_end;
uint16_t tx_seq_id;
PHOutputStream stream;
xSemaphoreHandle lock;
PHOutputStream output_stream;
PHDataHandler data_handler;
@ -75,13 +74,13 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
data->tx_seq_id = 0;
// Allocate the packet windows
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size);
data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size);
// Initialize the windows
data->tx_win_start = data->tx_win_end = 0;
data->rx_win_start = data->rx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
for (uint8_t i = 0; i < data->cfg.win_size; ++i)
{
data->tx_packets[i].header.type = PACKET_TYPE_NONE;
data->rx_packets[i].header.type = PACKET_TYPE_NONE;
@ -93,6 +92,12 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
// Initialize the ECC library.
initialize_ecc();
// Initialize the handlers
data->output_stream = 0;
data->data_handler = 0;
data->status_handler = 0;
data->ppm_handler = 0;
// Return the structure.
return (PHInstHandle)data;
}
@ -172,7 +177,7 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
// Find a free packet.
PHPacketHandle p = NULL;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
for (uint8_t i = 0; i < data->cfg.win_size; ++i)
if (data->tx_packets[i].header.type == PACKET_TYPE_NONE)
{
p = data->tx_packets + i;
@ -205,7 +210,7 @@ void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
// If this packet is at the start of the window, increment the start index.
while ((data->tx_win_start != data->tx_win_end) &&
(data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize;
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.win_size;
// Release lock
xSemaphoreGiveRecursive(data->lock);
@ -226,7 +231,7 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h)
// Find a free packet.
PHPacketHandle p = NULL;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
for (uint8_t i = 0; i < data->cfg.win_size; ++i)
if (data->rx_packets[i].header.type == PACKET_TYPE_NONE)
{
p = data->rx_packets + i;
@ -259,7 +264,7 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
// If this packet is at the start of the window, increment the start index.
while ((data->rx_win_start != data->rx_win_end) &&
(data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE))
data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize;
data->rx_win_start = (data->rx_win_start + 1) % data->cfg.win_size;
// Release lock
xSemaphoreGiveRecursive(data->lock);
@ -280,19 +285,39 @@ uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
if (!PHLTransmitPacket(data, p))
return 0;
// If this packet doesn't require an ACK, remove it from the TX window.
switch (p->header.type) {
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATA:
case PACKET_TYPE_PPM:
PHReleaseTXPacket(h, p);
break;
}
return 1;
}
/**
* Transmit a packet of data.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the data buffer.
* \param[in] len The length of the data buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Get a packet from the packet handler.
PHPacketHandle p = PHGetTXPacket(pios_packet_handler);
if (!p)
return 0;
// Initialize the packet.
p->header.destination_id = data->cfg.default_destination_id;
p->header.source_id = data->cfg.source_id;
p->header.type = PACKET_TYPE_DATA;
p->header.data_size = len;
// Copy the data into the packet.
memcpy(p->data, buf, len);
// Send the packet.
return PHLTransmitPacket(data, p);
}
/**
* Verify that a buffer contains a valid packet.
* \param[in] h The packet handler instance data pointer.
@ -380,11 +405,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
{
// Find the packet ID in the TX buffer, and free it.
unsigned int i = 0;
for (unsigned int i = 0; i < data->cfg.winSize; ++i)
for (unsigned int i = 0; i < data->cfg.win_size; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHReleaseTXPacket(h, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.winSize)
if (i == data->cfg.win_size)
DEBUG_PRINTF(1, "Error finding acked packet to release\n\r");
#endif
}
@ -394,11 +419,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
{
// Resend the packet.
unsigned int i = 0;
for ( ; i < data->cfg.winSize; ++i)
for ( ; i < data->cfg.win_size; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHLTransmitPacket(data, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.winSize)
if (i == data->cfg.win_size)
DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r");
DEBUG_PRINTF(1, "Resending after NACK\n\r");
#endif
@ -455,6 +480,7 @@ static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
// Transmit the packet using the output stream.
p->header.source_id = data->cfg.source_id;
if(data->output_stream(p) == -1)
return 0;

2
flight/Libraries/rscode/rs.c
View File

@ -159,7 +159,7 @@ debug_check_syndrome (void)
static void
compute_genpoly (int nbytes, int genpoly[])
{
int i, tp[256], tp1[256];
int i, tp[MAXDEG], tp1[MAXDEG];
/* multiply (x + a^n) for n = 1 to nbytes */

16
flight/Modules/Attitude/attitude.c
View File

@ -379,13 +379,13 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
if (GyrosReadOnly() || AccelsReadOnly())
return 0;
gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
accels[0] = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[1] = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
accels[2] = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
@ -420,6 +420,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyrosData->z += gyro_correct_int[2];
}
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += - gyrosData->z * yawBiasRate;
GyrosSet(gyrosData);
AccelsSet(accelsData);

333
flight/Modules/Autotune/autotune.c Normal file
View File

@ -0,0 +1,333 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Autotuning module
* @brief Reads from @ref ManualControlCommand and fakes a rate mode while
* toggling the channels to relay mode
* @{
*
* @file autotune.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeActual
*
* This module computes an attitude estimate from the sensor data
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "pios.h"
#include "flightstatus.h"
#include "hwsettings.h"
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include <pios_board_info.h>
// Private constants
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
// Private types
enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET};
// Private variables
static xTaskHandle taskHandle;
static bool autotuneEnabled;
// Private functions
static void AutotuneTask(void *parameters);
static void update_stabilization_settings();
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutotuneInitialize(void)
{
// Create a queue, connect to manual control command and flightstatus
#ifdef MODULE_AUTOTUNE_BUILTIN
autotuneEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED)
autotuneEnabled = true;
else
autotuneEnabled = false;
#endif
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutotuneStart(void)
{
// Start main task if it is enabled
if(autotuneEnabled) {
xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
}
return 0;
}
MODULE_INITCALL(AutotuneInitialize, AutotuneStart)
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
{
//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
enum AUTOTUNE_STATE state = AT_INIT;
portTickType lastUpdateTime = xTaskGetTickCount();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
// TODO:
// 1. get from queue
// 2. based on whether it is flightstatus or manualcontrol
portTickType diffTime;
const uint32_t PREPARE_TIME = 2000;
const uint32_t MEAURE_TIME = 30000;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only allow this module to run when autotuning
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
state = AT_INIT;
vTaskDelay(50);
continue;
}
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
ManualControlSettingsData manualSettings;
ManualControlSettingsGet(&manualSettings);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
if (rate) { // rate mode
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
switch(state) {
case AT_INIT:
lastUpdateTime = xTaskGetTickCount();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0)
state = AT_START;
break;
case AT_START:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Spend the first block of time in normal rate mode to get airborne
if (diffTime > PREPARE_TIME) {
state = AT_ROLL;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_ROLL:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the roll axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_PITCH;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_PITCH:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the pitch axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_FINISHED;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_FINISHED:
// Wait until disarmed and landed before updating the settings
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0)
state = AT_SET;
break;
case AT_SET:
update_stabilization_settings();
state = AT_INIT;
break;
default:
// Set an alarm or some shit like that
break;
}
StabilizationDesiredSet(&stabDesired);
vTaskDelay(10);
}
}
/**
* Called after measuring roll and pitch to update the
* stabilization settings
*
* takes in @ref RelayTuning and outputs @ref StabilizationSettings
*/
static void update_stabilization_settings()
{
RelayTuningData relayTuning;
RelayTuningGet(&relayTuning);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
// Eventually get these settings from RelayTuningSettings
const float gain_ratio_r = 1.0f / 3.0f;
const float zero_ratio_r = 1.0f / 3.0f;
const float gain_ratio_p = 1.0f / 5.0f;
const float zero_ratio_p = 1.0f / 5.0f;
// For now just run over roll and pitch
for (uint i = 0; i < 2; i++) {
float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s)
float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s)
float zc = wc * zero_ratio_r; // controller zero location (rad/s)
float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity
float kp = kpu * gain_ratio_r; // proportional gain
float ki = zc * kp; // integral gain
// Now calculate gains for the next loop out knowing it is the integral of
// the inner loop -- the plant is position/velocity = scale*1/s
float wc2 = wc * gain_ratio_p; // crossover of the attitude loop
float kp2 = wc2; // kp of attitude
float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude
switch(i) {
case 0: // roll
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
break;
case 1: // Pitch
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
break;
default:
// Oh shit oh shit oh shit
break;
}
}
switch(relaySettings.Behavior) {
case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
// Just measure, don't update the stab settings
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
StabilizationSettingsSet(&stabSettings);
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
StabilizationSettingsSet(&stabSettings);
UAVObjSave(StabilizationSettingsHandle(), 0);
break;
}
}
/**
* @}
* @}
*/

37
flight/Modules/Autotune/inc/autotune.h Normal file
View File

@ -0,0 +1,37 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Attitude Module
* @{
*
* @file attitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ATTITUDE_H
#define ATTITUDE_H
#include "openpilot.h"
int32_t AttitudeInitialize(void);
#endif // ATTITUDE_H

44
flight/Modules/Battery/battery.c
View File

@ -55,9 +55,6 @@
// Configuration
//
#define SAMPLE_PERIOD_MS 500
#define BATTERY_BOARD_VOLTAGE_WARNING 4.5
#define BATTERY_BOARD_VOLTAGE_CRITICAL 3.5
#define BATTERY_BOARD_VOLTAGE_ERROR 1.0
// Private types
// Private variables
@ -104,9 +101,6 @@ MODULE_INITCALL(BatteryInitialize, 0)
static void onTimer(UAVObjEvent* ev)
{
static FlightBatteryStateData flightBatteryData;
static bool BoardPowerWarning= false;
// prevent that the initial ramp up of the power supply rail is identified as a power failure.
static bool BoardPowerOk = false;
FlightBatterySettingsData batterySettings;
FlightBatterySettingsGet(&batterySettings);
@ -114,17 +108,11 @@ static void onTimer(UAVObjEvent* ev)
static float dT = SAMPLE_PERIOD_MS / 1000.0;
float energyRemaining;
if(HAS_SENSOR(FLIGHTBATTERYSETTINGS_SENSORTYPE_BOARDVOLTAGE) )
flightBatteryData.BoardSupplyVoltage=((float)PIOS_ADC_PinGet(4)) * PIOS_ADC_VOLTAGE_SCALE * 6.1;
else
flightBatteryData.BoardSupplyVoltage = -1;
//calculate the battery parameters
if(HAS_SENSOR(FLIGHTBATTERYSETTINGS_SENSORTYPE_BATTERYVOLTAGE) )
flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(0)) * PIOS_ADC_VOLTAGE_SCALE * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
else
flightBatteryData.Voltage = -1;
if(HAS_SENSOR(FLIGHTBATTERYSETTINGS_SENSORTYPE_BATTERYCURRENT))
{
@ -188,38 +176,6 @@ static void onTimer(UAVObjEvent* ev)
}
}
if(HAS_SENSOR(FLIGHTBATTERYSETTINGS_SENSORTYPE_BOARDVOLTAGE) )
{
// power ia disconnected from the board (it is powered by usb)
if(flightBatteryData.BoardSupplyVoltage!= -1 && flightBatteryData.BoardSupplyVoltage < BATTERY_BOARD_VOLTAGE_ERROR)
{
AlarmsSet(SYSTEMALARMS_ALARM_POWER, SYSTEMALARMS_ALARM_ERROR);
BoardPowerWarning=false;
BoardPowerOk = false;
}
else
{
if(BoardPowerOk && flightBatteryData.BoardSupplyVoltage < BATTERY_BOARD_VOLTAGE_CRITICAL)
{
AlarmsSet(SYSTEMALARMS_ALARM_POWER, SYSTEMALARMS_ALARM_CRITICAL);
BoardPowerWarning=true;
}
else if (BoardPowerOk && flightBatteryData.BoardSupplyVoltage < BATTERY_BOARD_VOLTAGE_WARNING)
{
AlarmsSet(SYSTEMALARMS_ALARM_POWER, SYSTEMALARMS_ALARM_WARNING);
BoardPowerWarning=true;
}
else
{
// if there was any previous warning/critical condition, notify the problem leaving the warning
if(BoardPowerWarning)
AlarmsSet(SYSTEMALARMS_ALARM_POWER, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsClear(SYSTEMALARMS_ALARM_POWER);
BoardPowerOk |= flightBatteryData.BoardSupplyVoltage > BATTERY_BOARD_VOLTAGE_WARNING;
}
}
}
FlightBatteryStateSet(&flightBatteryData);
}

5
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -32,7 +32,7 @@
#include "manualcontrolcommand.h"
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path;
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path;
#define PARSE_FLIGHT_MODE(x) ( \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
@ -43,7 +43,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();

10
flight/Modules/ManualControl/manualcontrol.c
View File

@ -419,6 +419,10 @@ static void manualControlTask(void *parameters)
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
@ -630,6 +634,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
@ -638,6 +644,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
@ -646,6 +654,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;

39
flight/Modules/Radio/inc/radio.h Normal file
View File

@ -0,0 +1,39 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Radio Input / Output Module
* @brief Read and Write packets from/to a radio device.
* @{
*
* @file radio.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Include file of the radio module.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef RADIOMODULE_H
#define RADIOMODULE_H
#endif // RADIOMODULE_H
/**
* @}
* @}
*/

550
flight/Modules/Radio/radio.c Normal file
View File

@ -0,0 +1,550 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Radio Input / Output Module
* @brief Read and Write packets from/to a radio device.
* @{
*
* @file radio.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Bridges selected Com Port to the COM VCP emulated serial port
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <gcsreceiver.h>
#include <hwsettings.h>
#include <pipxsettings.h>
#include <pipxstatus.h>
#include <packet_handler.h>
#include <pios_com_priv.h>
#include <pios_rfm22b_priv.h>
#include <radio.h>
// ****************
// Private constants
#define STACK_SIZE_BYTES 150
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
#define PACKET_QUEUE_SIZE PIOS_PH_WIN_SIZE
#define MAX_PORT_DELAY 200
#define STATS_UPDATE_PERIOD_MS 500
#define RADIOSTATS_UPDATE_PERIOD_MS 250
#define MAX_LOST_CONTACT_TIME 4
#ifndef LINK_LED_ON
#define LINK_LED_ON
#define LINK_LED_OFF
#endif
// ****************
// Private types
typedef struct {
uint32_t pairID;
uint16_t retries;
uint16_t errors;
uint16_t uavtalk_errors;
uint16_t resets;
uint16_t dropped;
int8_t rssi;
uint8_t lastContact;
} PairStats;
typedef struct {
// The task handles.
xTaskHandle radioReceiveTaskHandle;
xTaskHandle radioStatusTaskHandle;
xTaskHandle sendPacketTaskHandle;
// Queue handles.
xQueueHandle radioPacketQueue;
// Error statistics.
uint32_t radioTxErrors;
uint32_t radioRxErrors;
uint32_t packetErrors;
uint16_t txBytes;
uint16_t rxBytes;
// External error statistics
uint32_t droppedPackets;
uint32_t comTxRetries;
uint32_t UAVTalkErrors;
// The destination ID
uint32_t destination_id;
// Track other radios that are in range.
PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
// The RSSI of the last packet received.
int8_t RSSI;
} RadioData;
// ****************
// Private functions
static void radioReceiveTask(void *parameters);
static void radioStatusTask(void *parameters);
static void sendPacketTask(void *parameters);
static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
static int32_t transmitPacket(PHPacketHandle packet);
static void PPMHandler(uint16_t *channels);
// ****************
// Private variables
static RadioData *data = 0;
// ****************
// Global variables
uint32_t pios_rfm22b_id = 0;
uint32_t pios_com_rfm22b_id = 0;
uint32_t pios_packet_handler = 0;
extern struct pios_rfm22b_cfg pios_rfm22b_cfg;
/**
* Start the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t RadioStart(void)
{
if (!data)
return -1;
// Start the tasks.
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle));
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES * 2, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
xTaskCreate(sendPacketTask, (signed char *)"SendPacket", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->sendPacketTaskHandle));
// Install the monitors
TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, data->radioReceiveTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MODEMTX, data->sendPacketTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MODEMSTAT, data->radioStatusTaskHandle);
// Register the watchdog timers.
#ifdef PIOS_WDG_RADIORECEIVE
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
#endif /* PIOS_WDG_RADIORECEIVE */
return 0;
}
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t RadioInitialize(void)
{
// See if this module is enabled.
#ifndef RADIO_BUILTIN
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_RADIO] != HWSETTINGS_OPTIONALMODULES_ENABLED)
return -1;
#endif
// Initalize out UAVOs
PipXSettingsInitialize();
PipXStatusInitialize();
PipXSettingsData pipxSettings;
PipXSettingsGet(&pipxSettings);
/* Retrieve hardware settings. */
pios_rfm22b_cfg.frequencyHz = pipxSettings.Frequency;
pios_rfm22b_cfg.RFXtalCap = pipxSettings.FrequencyCalibration;
switch (pipxSettings.RFSpeed)
{
case PIPXSETTINGS_RFSPEED_2400:
pios_rfm22b_cfg.maxRFBandwidth = 2000;
break;
case PIPXSETTINGS_RFSPEED_4800:
pios_rfm22b_cfg.maxRFBandwidth = 4000;
break;
case PIPXSETTINGS_RFSPEED_9600:
pios_rfm22b_cfg.maxRFBandwidth = 9600;
break;
case PIPXSETTINGS_RFSPEED_19200:
pios_rfm22b_cfg.maxRFBandwidth = 19200;
break;
case PIPXSETTINGS_RFSPEED_38400:
pios_rfm22b_cfg.maxRFBandwidth = 32000;
break;
case PIPXSETTINGS_RFSPEED_57600:
pios_rfm22b_cfg.maxRFBandwidth = 64000;
break;
case PIPXSETTINGS_RFSPEED_115200:
pios_rfm22b_cfg.maxRFBandwidth = 128000;
break;
}
switch (pipxSettings.MaxRFPower)
{
case PIPXSETTINGS_MAXRFPOWER_125:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_0;
break;
case PIPXSETTINGS_MAXRFPOWER_16:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_1;
break;
case PIPXSETTINGS_MAXRFPOWER_316:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_2;
break;
case PIPXSETTINGS_MAXRFPOWER_63:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_3;
break;
case PIPXSETTINGS_MAXRFPOWER_126:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_4;
break;
case PIPXSETTINGS_MAXRFPOWER_25:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_5;
break;
case PIPXSETTINGS_MAXRFPOWER_50:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_6;
break;
case PIPXSETTINGS_MAXRFPOWER_100:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_7;
break;
}
/* Initalize the RFM22B radio COM device. */
{
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg.slave_num, &pios_rfm22b_cfg)) {
return -1;
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
// Initialize the packet handler
PacketHandlerConfig pios_ph_cfg = {
.default_destination_id = 0xffffffff, // Broadcast
.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id),
.win_size = PIOS_PH_WIN_SIZE,
.max_connections = PIOS_PH_MAX_CONNECTIONS,
};
pios_packet_handler = PHInitialize(&pios_ph_cfg);
// allocate and initialize the static data storage only if module is enabled
data = (RadioData *)pvPortMalloc(sizeof(RadioData));
if (!data)
return -1;
// Initialize the statistics.
data->radioTxErrors = 0;
data->radioRxErrors = 0;
data->packetErrors = 0;
data->droppedPackets = 0;
data->comTxRetries = 0;
data->UAVTalkErrors = 0;
data->RSSI = -127;
// Initialize the detected device statistics.
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
data->pairStats[i].pairID = 0;
data->pairStats[i].rssi = -127;
data->pairStats[i].retries = 0;
data->pairStats[i].errors = 0;
data->pairStats[i].uavtalk_errors = 0;
data->pairStats[i].resets = 0;
data->pairStats[i].dropped = 0;
data->pairStats[i].lastContact = 0;
}
// The first slot is reserved for our current pairID
PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
data->destination_id = data->pairStats[0].pairID ? data->pairStats[0].pairID : 0xffffffff;
// Create the packet queue.
data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle));
// Register the callbacks with the packet handler
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
return 0;
}
MODULE_INITCALL(RadioInitialize, RadioStart)
/**
* The task that receives packets from the radio.
*/
static void radioReceiveTask(void *parameters)
{
PHPacketHandle p = NULL;
/* Handle radio -> usart/usb direction */
while (1) {
uint32_t rx_bytes;
#ifdef PIOS_WDG_RADIORECEIVE
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
#endif /* PIOS_INCLUDE_WDG */
// Get a RX packet from the packet handler if required.
if (p == NULL)
p = PHGetRXPacket(pios_packet_handler);
if(p == NULL) {
// Wait a bit for a packet to come available.
vTaskDelay(5);
continue;
}
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
if(rx_bytes == 0)
continue;
data->rxBytes += rx_bytes;
// Verify that the packet is valid and pass it on.
bool rx_error = PHVerifyPacket(pios_packet_handler, p, rx_bytes) < 0;
if(rx_error)
data->packetErrors++;
PHReceivePacket(pios_packet_handler, p, rx_error);
p = NULL;
}
}
/**
* Send packets to the radio.
*/
static void sendPacketTask(void *parameters)
{
PHPacketHandle p;
// Loop forever
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->radioPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) {
PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p));
PHReleaseTXPacket(pios_packet_handler, p);
}
}
}
/**
* Transmit a packet to the radio port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitPacket(PHPacketHandle p)
{
uint16_t len = PH_PACKET_SIZE(p);
data->txBytes += len;
if (xQueueSend(data->radioPacketQueue, &p, portMAX_DELAY) != pdTRUE)
return -1;
return len;
}
/**
* Receive a status packet
* \param[in] status The status structure
*/
static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc)
{
uint32_t id = status->header.source_id;
bool found = false;
// Have we seen this device recently?
uint8_t id_idx = 0;
for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
if(data->pairStats[id_idx].pairID == id)
{
found = true;
break;
}
// If we have seen it, update the RSSI and reset the last contact couter
if(found)
{
data->pairStats[id_idx].rssi = rssi;
data->pairStats[id_idx].retries = status->retries;
data->pairStats[id_idx].errors = status->errors;
data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[id_idx].resets = status->resets;
data->pairStats[id_idx].dropped = status->dropped;
data->pairStats[id_idx].lastContact = 0;
}
// If we haven't seen it, find a slot to put it in.
if (!found)
{
uint32_t pairID;
PipXSettingsPairIDGet(&pairID);
uint8_t min_idx = 0;
if(id != pairID)
{
int8_t min_rssi = data->pairStats[0].rssi;
for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
{
if(data->pairStats[id_idx].rssi < min_rssi)
{
min_rssi = data->pairStats[id_idx].rssi;
min_idx = id_idx;
}
}
}
data->pairStats[min_idx].pairID = id;
data->pairStats[min_idx].rssi = rssi;
data->pairStats[min_idx].retries = status->retries;
data->pairStats[min_idx].errors = status->errors;
data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[min_idx].resets = status->resets;
data->pairStats[min_idx].dropped = status->dropped;
data->pairStats[min_idx].lastContact = 0;
}
}
/**
* The stats update task.
*/
static void radioStatusTask(void *parameters)
{
static portTickType lastSysTime;
PHStatusPacket status_packet;
while (1) {
lastSysTime = xTaskGetTickCount();
PipXStatusData pipxStatus;
uint32_t pairID;
// Get object data
PipXStatusGet(&pipxStatus);
PipXSettingsPairIDGet(&pairID);
// Update the status
pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
pipxStatus.Retries = data->comTxRetries;
pipxStatus.Errors = data->packetErrors;
pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
pipxStatus.Dropped = data->droppedPackets;
pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->txBytes = 0;
pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->rxBytes = 0;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
pipxStatus.RSSI = data->RSSI;
LINK_LED_OFF;
// Update the potential pairing contacts
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
data->pairStats[i].lastContact++;
// Remove this device if it's stale.
if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME)
{
data->pairStats[i].pairID = 0;
data->pairStats[i].rssi = -127;
data->pairStats[i].retries = 0;
data->pairStats[i].errors = 0;
data->pairStats[i].uavtalk_errors = 0;
data->pairStats[i].resets = 0;
data->pairStats[i].dropped = 0;
data->pairStats[i].lastContact = 0;
}
// Add the paired devices statistics to ours.
if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127))
{
pipxStatus.Retries += data->pairStats[i].retries;
pipxStatus.Errors += data->pairStats[i].errors;
pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
pipxStatus.Dropped += data->pairStats[i].dropped;
pipxStatus.Resets += data->pairStats[i].resets;
pipxStatus.Dropped += data->pairStats[i].dropped;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
LINK_LED_ON;
}
}
// Update the object
PipXStatusSet(&pipxStatus);
// Broadcast the status.
{
static uint16_t cntr = 0;
if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
{
// Queue the status message
status_packet.header.destination_id = 0xffffffff;
status_packet.header.type = PACKET_TYPE_STATUS;
status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet);
status_packet.header.source_id = pipxStatus.DeviceID;
status_packet.retries = data->comTxRetries;
status_packet.errors = data->packetErrors;
status_packet.uavtalk_errors = data->UAVTalkErrors;
status_packet.dropped = data->droppedPackets;
status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
PHPacketHandle sph = (PHPacketHandle)&status_packet;
PHTransmitPacket(PIOS_PACKET_HANDLER, sph);
cntr = 0;
}
}
portTickType timeSinceUpdate;
do {
PIOS_RFM22_processPendingISR(5);
timeSinceUpdate = xTaskGetTickCount() - lastSysTime;
}
while(timeSinceUpdate < STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* Receive a ppm packet
* \param[in] channels The ppm channels
*/
static void PPMHandler(uint16_t *channels)
{
GCSReceiverData rcvr;
// Copy the receiver channels into the GCSReceiver object.
for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
rcvr.Channel[i] = channels[i];
// Set the GCSReceiverData object.
GCSReceiverSet(&rcvr);
}

448
flight/Modules/RadioComBridge/RadioComBridge.c
View File

@ -55,8 +55,8 @@
#define BRIDGE_BUF_LEN 512
#define MAX_RETRIES 2
#define RETRY_TIMEOUT_MS 20
#define STATS_UPDATE_PERIOD_MS 500
#define RADIOSTATS_UPDATE_PERIOD_MS 250
#define STATS_UPDATE_PERIOD_MS 1000
#define RADIOSTATS_UPDATE_PERIOD_MS 500
#define MAX_LOST_CONTACT_TIME 4
#define PACKET_QUEUE_SIZE 10
#define MAX_PORT_DELAY 200
@ -68,25 +68,13 @@
// ****************
// Private types
typedef struct {
uint32_t pairID;
uint16_t retries;
uint16_t errors;
uint16_t uavtalk_errors;
uint16_t resets;
uint16_t dropped;
int8_t rssi;
uint8_t lastContact;
} PairStats;
typedef struct {
uint32_t comPort;
UAVTalkConnection UAVTalkCon;
xQueueHandle sendQueue;
xQueueHandle recvQueue;
xQueueHandle gcsQueue;
uint16_t wdg;
bool checkHID;
bool isGCS;
} UAVTalkComTaskParams;
typedef struct {
@ -94,10 +82,7 @@ typedef struct {
// The task handles.
xTaskHandle GCSUAVTalkRecvTaskHandle;
xTaskHandle UAVTalkRecvTaskHandle;
xTaskHandle radioReceiveTaskHandle;
xTaskHandle sendPacketTaskHandle;
xTaskHandle UAVTalkSendTaskHandle;
xTaskHandle radioStatusTaskHandle;
xTaskHandle transparentCommTaskHandle;
xTaskHandle ppmInputTaskHandle;
@ -106,23 +91,14 @@ typedef struct {
UAVTalkConnection GCSUAVTalkCon;
// Queue handles.
xQueueHandle radioPacketQueue;
xQueueHandle gcsEventQueue;
xQueueHandle uavtalkEventQueue;
xQueueHandle ppmOutQueue;
// Error statistics.
uint32_t comTxErrors;
uint32_t comTxRetries;
uint32_t comRxErrors;
uint32_t radioTxErrors;
uint32_t radioTxRetries;
uint32_t radioRxErrors;
uint32_t UAVTalkErrors;
uint32_t packetErrors;
uint32_t droppedPackets;
uint16_t txBytes;
uint16_t rxBytes;
// The destination ID
uint32_t destination_id;
@ -131,9 +107,6 @@ typedef struct {
portTickType send_timeout;
uint16_t min_packet_size;
// Track other radios that are in range.
PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
// The RSSI of the last packet received.
int8_t RSSI;
@ -155,19 +128,13 @@ typedef struct {
// Private functions
static void UAVTalkRecvTask(void *parameters);
static void radioReceiveTask(void *parameters);
static void sendPacketTask(void *parameters);
static void UAVTalkSendTask(void *parameters);
static void transparentCommTask(void * parameters);
static void radioStatusTask(void *parameters);
static void ppmInputTask(void *parameters);
static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length);
static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length);
static int32_t transmitPacket(PHPacketHandle packet);
static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid);
static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
static void PPMHandler(uint16_t *channels);
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
@ -187,6 +154,11 @@ static RadioComBridgeData *data;
static int32_t RadioComBridgeStart(void)
{
if(data) {
// Register the callbacks with the packet handler
// This has to happen after the Radio module is initialized.
PHRegisterDataHandler(pios_packet_handler, receiveData);
// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle));
xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
@ -202,19 +174,15 @@ static int32_t RadioComBridgeStart(void)
// Start the tasks
if(PIOS_COM_TRANS_COM)
xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle));
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle));
xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle));
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
if(PIOS_PPM_RECEIVER)
xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle));
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS);
if(PIOS_COM_UAVTALK)
PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
if(PIOS_COM_TRANS_COM)
PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
if(PIOS_PPM_RECEIVER)
PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
#endif
@ -247,76 +215,47 @@ static int32_t RadioComBridgeInitialize(void)
data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler);
if (PIOS_COM_UAVTALK)
data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
else
data->UAVTalkCon = 0;
// Initialize the queues.
data->ppmOutQueue = 0;
data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
if (PIOS_COM_UAVTALK)
data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
else
{
data->uavtalkEventQueue = 0;
data->ppmOutQueue = data->radioPacketQueue;
}
// Initialize the statistics.
data->radioTxErrors = 0;
data->radioTxRetries = 0;
data->radioRxErrors = 0;
data->comTxErrors = 0;
data->comTxRetries = 0;
data->comRxErrors = 0;
data->UAVTalkErrors = 0;
data->packetErrors = 0;
data->RSSI = -127;
// Register the callbacks with the packet handler
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
PHRegisterDataHandler(pios_packet_handler, receiveData);
PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
// Initialize the packet send timeout
data->send_timeout = 25; // ms
data->min_packet_size = 50;
// Initialize the detected device statistics.
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
data->pairStats[i].pairID = 0;
data->pairStats[i].rssi = -127;
data->pairStats[i].retries = 0;
data->pairStats[i].errors = 0;
data->pairStats[i].uavtalk_errors = 0;
data->pairStats[i].resets = 0;
data->pairStats[i].dropped = 0;
data->pairStats[i].lastContact = 0;
}
// The first slot is reserved for our current pairID
PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
// Initialize the UAVTalk comm parameters.
data->gcs_uavtalk_params.isGCS = true;
data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon;
data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue;
data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue;
data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS;
data->gcs_uavtalk_params.checkHID = true;
data->gcs_uavtalk_params.comPort = PIOS_COM_GCS;
if (PIOS_COM_UAVTALK)
{
data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
data->uavtalk_params.isGCS = false;
data->uavtalk_params.UAVTalkCon = data->UAVTalkCon;
data->uavtalk_params.sendQueue = data->radioPacketQueue;
data->uavtalk_params.sendQueue = data->gcsEventQueue;
data->uavtalk_params.recvQueue = data->uavtalkEventQueue;
data->uavtalk_params.gcsQueue = data->gcsEventQueue;
data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK;
data->uavtalk_params.checkHID = false;
data->uavtalk_params.comPort = PIOS_COM_UAVTALK;
}
@ -336,6 +275,8 @@ static void UAVTalkRecvTask(void *parameters)
BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE);
while (1) {
bool HID_available = false;
xQueueHandle sendQueue = 0;
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
@ -343,13 +284,16 @@ static void UAVTalkRecvTask(void *parameters)
PIOS_WDG_UpdateFlag(params->wdg);
#endif /* PIOS_INCLUDE_WDG */
// Receive from USB HID if available, otherwise UAVTalk com if it's available.
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (params->checkHID && PIOS_USB_CheckAvailable(0))
// Is USB plugged in?
if (PIOS_USB_CheckAvailable(0))
HID_available = true;
#endif /* PIOS_INCLUDE_USB */
// Receive from USB HID if available, otherwise UAVTalk com if it's available.
if (params->isGCS && HID_available)
BufferedReadSetCom(f, PIOS_COM_USB_HID);
else
#endif /* PIOS_INCLUDE_USB */
{
if (params->comPort)
BufferedReadSetCom(f, params->comPort);
@ -360,9 +304,14 @@ static void UAVTalkRecvTask(void *parameters)
}
}
// Send packets to the UAVTalk port if this is a GCS connction and UAVTalk port is configured
// or to the GCS port if this is a UAVTalk connection and USB is plugged in.
if ((params->isGCS && data->UAVTalkCon) || (!params->isGCS && HID_available))
sendQueue = params->sendQueue;
// Read the next byte
uint8_t rx_byte;
if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
if (!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
continue;
// Get a TX packet from the packet handler if required.
@ -386,7 +335,6 @@ static void UAVTalkRecvTask(void *parameters)
// Initialize the packet.
p->header.destination_id = data->destination_id;
p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
p->header.type = PACKET_TYPE_DATA;
p->data[0] = rx_byte;
p->header.data_size = 1;
@ -403,13 +351,6 @@ static void UAVTalkRecvTask(void *parameters)
if (state == UAVTALK_STATE_COMPLETE)
{
xQueueHandle sendQueue = params->sendQueue;
#if defined(PIOS_INCLUDE_USB)
if (params->gcsQueue)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
sendQueue = params->gcsQueue;
#endif /* PIOS_INCLUDE_USB */
// Is this a local UAVObject?
// We only generate GcsReceiver ojects, we don't consume them.
if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID))
@ -486,7 +427,10 @@ static void UAVTalkRecvTask(void *parameters)
else
{
// Otherwise, queue the packet for transmission.
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
if (sendQueue)
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
else
PHTransmitPacket(PIOS_PACKET_HANDLER, p);
}
}
else
@ -515,17 +459,14 @@ static void UAVTalkRecvTask(void *parameters)
else
{
// Queue the packet for transmission.
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
if (sendQueue)
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
else
PHTransmitPacket(PIOS_PACKET_HANDLER, p);
}
p = NULL;
} else if(state == UAVTALK_STATE_ERROR) {
xQueueHandle sendQueue = params->sendQueue;
#if defined(PIOS_INCLUDE_USB)
if (params->gcsQueue)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
sendQueue = params->gcsQueue;
#endif /* PIOS_INCLUDE_USB */
data->UAVTalkErrors++;
// Send a NACK if required.
@ -540,84 +481,16 @@ static void UAVTalkRecvTask(void *parameters)
else
{
// Transmit the packet anyway...
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
if (sendQueue)
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
else
PHTransmitPacket(PIOS_PACKET_HANDLER, p);
}
p = NULL;
}
}
}
/**
* The radio to com bridge task.
*/
static void radioReceiveTask(void *parameters)
{
PHPacketHandle p = NULL;
/* Handle radio -> usart/usb direction */
while (1) {
uint32_t rx_bytes;
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
#endif /* PIOS_INCLUDE_WDG */
// Get a RX packet from the packet handler if required.
if (p == NULL)
p = PHGetRXPacket(pios_packet_handler);
if(p == NULL) {
DEBUG_PRINTF(2, "RX Packet Unavailable.!\n\r");
// Wait a bit for a packet to come available.
vTaskDelay(5);
continue;
}
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
if(rx_bytes == 0)
continue;
data->rxBytes += rx_bytes;
// Verify that the packet is valid and pass it on.
if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) {
UAVObjEvent ev;
ev.obj = (UAVObjHandle)p;
ev.event = EV_PACKET_RECEIVED;
xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY);
} else
{
data->packetErrors++;
PHReceivePacket(pios_packet_handler, p, true);
}
p = NULL;
}
}
/**
* Send packets to the radio.
*/
static void sendPacketTask(void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->radioPacketQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
PHPacketHandle p = (PHPacketHandle)ev.obj;
// Send the packet.
if(!PHTransmitPacket(pios_packet_handler, p))
PHReleaseRXPacket(pios_packet_handler, p);
}
}
}
/**
* Send packets to the com port.
*/
@ -681,7 +554,7 @@ static void UAVTalkSendTask(void *parameters)
{
// Transmit the packet.
PHPacketHandle p = (PHPacketHandle)ev.obj;
transmitData(params->comPort, p->data, p->header.data_size, params->checkHID);
UAVTalkSendBuf(params->UAVTalkCon, p->data, p->header.data_size);
PHReleaseTXPacket(pios_packet_handler, p);
}
}
@ -721,7 +594,6 @@ static void transparentCommTask(void * parameters)
// Initialize the packet.
p->header.destination_id = data->destination_id;
p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
//p->header.type = PACKET_TYPE_ACKED_DATA;
p->header.type = PACKET_TYPE_DATA;
p->header.data_size = 0;
@ -760,7 +632,7 @@ static void transparentCommTask(void * parameters)
if (send_packet)
{
// Queue the packet for transmission.
queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
PHTransmitPacket(PIOS_PACKET_HANDLER, p);
// Reset the timeout
timeout = MAX_PORT_DELAY;
@ -771,97 +643,6 @@ static void transparentCommTask(void * parameters)
}
}
/**
* The stats update task.
*/
static void radioStatusTask(void *parameters)
{
PHStatusPacket status_packet;
while (1) {
PipXStatusData pipxStatus;
uint32_t pairID;
// Get object data
PipXStatusGet(&pipxStatus);
PipXSettingsPairIDGet(&pairID);
// Update the status
pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
pipxStatus.Retries = data->comTxRetries;
pipxStatus.Errors = data->packetErrors;
pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
pipxStatus.Dropped = data->droppedPackets;
pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->txBytes = 0;
pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->rxBytes = 0;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
pipxStatus.RSSI = data->RSSI;
LINK_LED_OFF;
// Update the potential pairing contacts
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
data->pairStats[i].lastContact++;
// Remove this device if it's stale.
if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME)
{
data->pairStats[i].pairID = 0;
data->pairStats[i].rssi = -127;
data->pairStats[i].retries = 0;
data->pairStats[i].errors = 0;
data->pairStats[i].uavtalk_errors = 0;
data->pairStats[i].resets = 0;
data->pairStats[i].dropped = 0;
data->pairStats[i].lastContact = 0;
}
// Add the paired devices statistics to ours.
if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127))
{
pipxStatus.Retries += data->pairStats[i].retries;
pipxStatus.Errors += data->pairStats[i].errors;
pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
pipxStatus.Dropped += data->pairStats[i].dropped;
pipxStatus.Resets += data->pairStats[i].resets;
pipxStatus.Dropped += data->pairStats[i].dropped;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
LINK_LED_ON;
}
}
// Update the object
PipXStatusSet(&pipxStatus);
// Broadcast the status.
{
static uint16_t cntr = 0;
if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
{
// Queue the status message
status_packet.header.destination_id = 0xffffffff;
status_packet.header.type = PACKET_TYPE_STATUS;
status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet);
status_packet.header.source_id = pipxStatus.DeviceID;
status_packet.retries = data->comTxRetries;
status_packet.errors = data->packetErrors;
status_packet.uavtalk_errors = data->UAVTalkErrors;
status_packet.dropped = data->droppedPackets;
status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
PHPacketHandle sph = (PHPacketHandle)&status_packet;
queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET);
cntr = 0;
}
}
// Delay until the next update period.
vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* The PPM input task.
*/
@ -878,20 +659,38 @@ static void ppmInputTask(void *parameters)
#endif /* PIOS_INCLUDE_WDG */
// Read the receiver.
bool valid_input_detected = false;
for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i)
ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
// Send the PPM packet
if (data->ppmOutQueue)
{
ppm_packet.header.destination_id = data->destination_id;
ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
ppm_packet.header.type = PACKET_TYPE_PPM;
ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET);
ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
if(ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT)
valid_input_detected = true;
}
// Send the PPM packet if it's valid
if (valid_input_detected)
{
// Set the GCSReceiver UAVO if we're connected to the FC.
if (data->UAVTalkCon)
{
GCSReceiverData rcvr;
// Copy the receiver channels into the GCSReceiver object.
for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
rcvr.Channel[i] = ppm_packet.channels[i];
// Set the GCSReceiverData object.
GCSReceiverSet(&rcvr);
}
else
{
// Otherwise, send a PPM packet over the radio link.
ppm_packet.header.destination_id = data->destination_id;
ppm_packet.header.type = PACKET_TYPE_PPM;
ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
PHTransmitPacket(PIOS_PACKET_HANDLER, pph);
}
}
else
PPMHandler(ppm_packet.channels);
// Delay until the next update period.
vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS);
@ -910,7 +709,7 @@ static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t l
{
uint32_t outputPort = params->comPort;
#if defined(PIOS_INCLUDE_USB)
if (params->checkHID)
if (params->isGCS)
{
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
@ -947,19 +746,6 @@ static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length)
return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length);
}
/**
* Transmit a packet to the radio port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitPacket(PHPacketHandle p)
{
data->txBytes += PH_PACKET_SIZE(p);
return PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p));
}
/**
* Receive a packet
* \param[in] buf The received data buffer
@ -993,85 +779,13 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
data->RSSI = rssi;
// Packet data should go to transparent com if it's configured,
// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK;
bool checkHid = (PIOS_COM_TRANS_COM == 0);
transmitData(outputPort, buf, len, checkHid);
}
/**
* Receive a status packet
* \param[in] status The status structure
*/
static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc)
{
uint32_t id = status->header.source_id;
bool found = false;
// Have we seen this device recently?
uint8_t id_idx = 0;
for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
if(data->pairStats[id_idx].pairID == id)
{
found = true;
break;
}
// If we have seen it, update the RSSI and reset the last contact couter
if(found)
{
data->pairStats[id_idx].rssi = rssi;
data->pairStats[id_idx].retries = status->retries;
data->pairStats[id_idx].errors = status->errors;
data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[id_idx].resets = status->resets;
data->pairStats[id_idx].dropped = status->dropped;
data->pairStats[id_idx].lastContact = 0;
}
// If we haven't seen it, find a slot to put it in.
if (!found)
{
uint32_t pairID;
PipXSettingsPairIDGet(&pairID);
uint8_t min_idx = 0;
if(id != pairID)
{
int8_t min_rssi = data->pairStats[0].rssi;
for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
{
if(data->pairStats[id_idx].rssi < min_rssi)
{
min_rssi = data->pairStats[id_idx].rssi;
min_idx = id_idx;
}
}
}
data->pairStats[min_idx].pairID = id;
data->pairStats[min_idx].rssi = rssi;
data->pairStats[min_idx].retries = status->retries;
data->pairStats[min_idx].errors = status->errors;
data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[min_idx].resets = status->resets;
data->pairStats[min_idx].dropped = status->dropped;
data->pairStats[min_idx].lastContact = 0;
}
}
/**
* Receive a ppm packet
* \param[in] channels The ppm channels
*/
static void PPMHandler(uint16_t *channels)
{
GCSReceiverData rcvr;
// Copy the receiver channels into the GCSReceiver object.
for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
rcvr.Channel[i] = channels[i];
// Set the GCSReceiverData object.
GCSReceiverSet(&rcvr);
// or just send it through the UAVTalk link.
if (PIOS_COM_TRANS_COM)
transmitData(PIOS_COM_TRANS_COM, buf, len, false);
else if (data->UAVTalkCon)
UAVTalkSendBuf(data->UAVTalkCon, buf, len);
else
UAVTalkSendBuf(data->GCSUAVTalkCon, buf, len);
}
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length)

25
flight/Modules/Sensors/sensors.c
View File

@ -47,7 +47,6 @@
*/
#include "pios.h"
#include "attitude.h"
#include "homelocation.h"
#include "magnetometer.h"
#include "magbias.h"
@ -260,9 +259,9 @@ static void SensorsTask(void *parameters)
while(read_good == 0) {
count++;
accel_accum[0] += accel.x;
accel_accum[1] += accel.y;
accel_accum[2] += accel.z;
accel_accum[1] += accel.x;
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
@ -285,9 +284,9 @@ static void SensorsTask(void *parameters)
}
gyro_samples = 1;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_accum[1] += gyro.gyro_x;
gyro_accum[0] += gyro.gyro_y;
gyro_accum[2] -= gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
@ -335,9 +334,9 @@ static void SensorsTask(void *parameters)
}
// Scale the accels
float accels[3] = {(float) accel_accum[1] / accel_samples,
(float) accel_accum[0] / accel_samples,
-(float) accel_accum[2] / accel_samples};
float accels[3] = {(float) accel_accum[0] / accel_samples,
(float) accel_accum[1] / accel_samples,
(float) accel_accum[2] / accel_samples};
float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
@ -354,9 +353,9 @@ static void SensorsTask(void *parameters)
AccelsSet(&accelsData);
// Scale the gyros
float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[0] / gyro_samples,
-(float) gyro_accum[2] / gyro_samples};
float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};

39
flight/Modules/Stabilization/inc/relay_tuning.h Normal file
View File

@ -0,0 +1,39 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file relay_tuning.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef RELAY_TUNING_H
#define RELAY_TUNING_H
int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
#endif

2
flight/Modules/Stabilization/inc/stabilization.h
View File

@ -33,6 +33,8 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum {ROLL,PITCH,YAW,MAX_AXES};
int32_t StabilizationInitialize();
#endif // STABILIZATION_H

42
flight/Modules/Stabilization/inc/virtualflybar.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Virtual flybar mode
* @note This file implements the logic for a virtual flybar
* @{
*
* @file virtualflybar.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef VIRTUALFLYBAR_H
#define VIRTUALFLYBAR_H
#include "openpilot.h"
#include "stabilizationsettings.h"
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings);
int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT);
#endif /* VIRTUALFLYBAR_H */

151
flight/Modules/Stabilization/relay_tuning.c Normal file
View File

@ -0,0 +1,151 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "sin_lookup.h"
//! Private variables
static const int SIN_RESOLUTION = 180;
#define MAX_AXES 3
/**
* Apply a step function for the stabilization controller and monitor the
* result
*
* Used to Replace the rate PID with a relay to measure the critical properties of this axis
* i.e. period and gain
*/
int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
{
RelayTuningData relay;
RelayTuningGet(&relay);
static portTickType lastHighTime;
static portTickType lastLowTime;
static float accum_sin, accum_cos;
static uint32_t accumulated = 0;
const uint16_t DEGLITCH_TIME = 20; // ms
const float AMPLITUDE_ALPHA = 0.95;
const float PERIOD_ALPHA = 0.95;
portTickType thisTime = xTaskGetTickCount();
static bool rateRelayRunning[MAX_AXES];
// This indicates the current estimate of the smoothed error. So when it is high
// we are waiting for it to go low.
static bool high = false;
// On first run initialize estimates to something reasonable
if(reinit) {
rateRelayRunning[axis] = false;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
accum_sin = 0;
accum_cos = 0;
accumulated = 0;
// These should get reinitialized anyway
high = true;
lastHighTime = thisTime;
lastLowTime = thisTime;
RelayTuningSet(&relay);
}
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
// Compute output, simple threshold on error
*output = high ? relaySettings.Amplitude : -relaySettings.Amplitude;
/**** The code below here is to estimate the properties of the oscillation ****/
// Make sure the period can't go below limit
if (relay.Period[axis] < DEGLITCH_TIME)
relay.Period[axis] = DEGLITCH_TIME;
// Project the error onto a sine and cosine of the same frequency
// to accumulate the average amplitude
int32_t dT = thisTime - lastHighTime;
float phase = ((float)360 * (float)dT) / relay.Period[axis];
if(phase >= 360)
phase = 0;
accum_sin += sin_lookup_deg(phase) * error;
accum_cos += sin_lookup_deg(phase + 90) * error;
accumulated ++;
// Make sure we've had enough time since last transition then check for a change in the output
bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){
/* POSITIVE CROSSING DETECTED */
float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
float this_gain = this_amplitude / relaySettings.Amplitude;
accumulated = 0;
accum_sin = 0;
accum_cos = 0;
if(rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
} else {
// Low pass filter each amplitude and period
relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
}
lastHighTime = thisTime;
high = true;
RelayTuningSet(&relay);
} else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) {
/* FALLING CROSSING DETECTED */
lastLowTime = thisTime;
high = false;
}
return 0;
}

430
flight/Modules/Stabilization/stabilization.c
View File

@ -36,12 +36,22 @@
#include "stabilizationsettings.h"
#include "actuatordesired.h"
#include "ratedesired.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudeactual.h"
#include "gyros.h"
#include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro
// Math libraries
#include "CoordinateConversions.h"
#include "pid.h"
#include "sin_lookup.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
#include "virtualflybar.h"
// Private constants
#define MAX_QUEUE_SIZE 1
@ -55,44 +65,26 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define FAILSAFE_TIMEOUT_MS 30
enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX};
enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX};
enum {ROLL,PITCH,YAW,MAX_AXES};
// Private types
typedef struct {
float p;
float i;
float d;
float iLim;
float iAccumulator;
float lastErr;
} pid_type;
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float gyro_alpha = 0;
float gyro_filtered[3] = {0,0,0};
float axis_lock_accum[3] = {0,0,0};
float vbar_sensitivity[3] = {1, 1, 1};
uint8_t max_axis_lock = 0;
uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
float vbar_integral[3] = {0, 0, 0};
float vbar_decay = 0.991f;
pid_type pids[PID_MAX];
int8_t vbar_gyros_suppress;
bool vbar_piro_comp = false;
struct pid pids[PID_MAX];
// Private functions
static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float err, float dT);
static float bound(float val);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev);
@ -108,10 +100,10 @@ int32_t StabilizationStart()
// Listen for updates.
// AttitudeActualConnectQueue(queue);
GyrosConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -131,6 +123,11 @@ int32_t StabilizationInitialize()
RateDesiredInitialize();
#endif
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
RelayTuningInitialize();
return 0;
}
@ -142,73 +139,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
GyrosData gyrosData;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
ZeroPids();
while(1) {
float dT;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
#if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired);
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch;
else
rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
@ -217,7 +214,7 @@ static void stabilizationTask(void* parameters)
local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
#endif
float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
@ -226,49 +223,79 @@ static void stabilizationTask(void* parameters)
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate
ActuatorDesiredGet(&actuatorDesired);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = {255,255,255};
bool error = false;
//Run the selected stabilization algorithm on each axis:
for(uint8_t i=0; i< MAX_AXES; i++)
{
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode[i];
// Apply the selected control law
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if(reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if(reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = attitudeDesiredAxis[i];
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i].iAccumulator = 0;
axis_lock_accum[i] = 0;
break;
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
float weak_leveling = local_error[i] * weak_leveling_kp;
weak_leveling = bound(weak_leveling, weak_leveling_max);
if(weak_leveling > weak_leveling_max)
weak_leveling = weak_leveling_max;
if(weak_leveling < -weak_leveling_max)
weak_leveling = -weak_leveling_max;
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i].iAccumulator = 0;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
rateDesiredAxis[i] = -settings.MaximumRate[i];
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
@ -276,154 +303,96 @@ static void stabilizationTask(void* parameters)
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
rateDesiredAxis[i] = -settings.MaximumRate[i];
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
if(reinit)
pids[PID_ROLL + i].iAccumulator = 0;
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
break;
default:
error = true;
break;
}
}
// Piro compensation rotates the virtual flybar by yaw step to keep it
// rotated to external world
if (vbar_piro_comp) {
const float F_PI = 3.14159f;
float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
vbar_integral[1] = vbar_pitch;
vbar_integral[0] = vbar_roll;
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
uint8_t shouldUpdate = 1;
#if defined(DIAGNOSTICS)
RateDesiredSet(&rateDesired);
#endif
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(uint32_t i = 0; i < MAX_AXES; i++)
{
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
{
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
if (vbar_integral[i] > settings.VbarMaxAngle)
vbar_integral[i] = settings.VbarMaxAngle;
if (-vbar_integral[i] < -settings.VbarMaxAngle)
vbar_integral[i] = -settings.VbarMaxAngle;
// Command signal is composed of stick input added to the gyro and virtual flybar
float gyro_gain = 1.0f;
if (vbar_gyros_suppress > 0) {
gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f);
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
}
float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * (
vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p);
actuatorDesiredAxis[i] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
switch (i)
{
case ROLL:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_ROLL].iAccumulator = 0;
pids[PID_ROLL].iAccumulator = 0;
break;
case PITCH:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_PITCH].iAccumulator = 0;
pids[PID_PITCH].iAccumulator = 0;
break;
case YAW:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_YAW].iAccumulator = 0;
pids[PID_YAW].iAccumulator = 0;
break;
}
break;
}
}
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
if(dT > 15)
actuatorDesired.NumLongUpdates++;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
(lowThrottleZeroIntegral && stabDesired.Throttle < 0))
{
ZeroPids();
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Clear or set alarms. Done like this to prevent toggline each cycle
// and hammering system alarms
if (error)
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
float ApplyPid(pid_type * pid, const float err, float dT)
{
float diff = (err - pid->lastErr);
pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
pid->iAccumulator = pid->iLim * 1000.0f;
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
pid->iAccumulator = -pid->iLim * 1000.0f;
}
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
}
/**
* Clear the accumulators and derivatives for all the axes
*/
static void ZeroPids(void)
{
for(int8_t ct = 0; ct < PID_MAX; ct++) {
pids[ct].iAccumulator = 0.0f;
pids[ct].lastErr = 0.0f;
}
for(uint32_t i = 0; i < PID_MAX; i++)
pid_zero(&pids[i]);
for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0.0f;
}
@ -432,12 +401,12 @@ static void ZeroPids(void)
/**
* Bound input value between limits
*/
static float bound(float val)
static float bound(float val, float range)
{
if(val < -1.0f) {
val = -1.0f;
} else if(val > 1.0f) {
val = 1.0f;
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}
@ -445,68 +414,54 @@ static float bound(float val)
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
// Set the pitch rate PID constants
pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP],
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
// Set the yaw rate PID constants
pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP],
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
// Set the roll attitude PI constants
pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
// Set the pitch attitude PI constants
pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
// Set the yaw attitude PI constants
pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Set the roll attitude PI constants
pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
// Set the pitch attitude PI constants
pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP];
pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI];
pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
// Set the yaw attitude PI constants
pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
// Need to store the vbar sensitivity
vbar_sensitivity[0] = settings.VbarSensitivity[0];
vbar_sensitivity[1] = settings.VbarSensitivity[1];
vbar_sensitivity[2] = settings.VbarSensitivity[2];
pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
@ -517,11 +472,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = expf(-fakeDt / settings.GyroTau);
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
vbar_gyros_suppress = settings.VbarGyroSuppress;
vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE;
}
@ -529,3 +482,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
* @}
* @}
*/

119
flight/Modules/Stabilization/virtualflybar.c Normal file
View File

@ -0,0 +1,119 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Virtual flybar mode
* @note This file implements the logic for a virtual flybar
* @{
*
* @file virtualflybar.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "stabilization.h"
#include "stabilizationsettings.h"
//! Private variables
static float vbar_integral[MAX_AXES];
static float vbar_decay = 0.991f;
//! Private methods
static float bound(float val, float range);
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
{
float gyro_gain = 1.0f;
float kp = 0, ki = 0;
if(reinit)
vbar_integral[axis] = 0;
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) {
gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f);
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
}
// Get the settings for the correct axis
switch(axis)
{
case ROLL:
kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
case PITCH:
kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
case YAW:
kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
default:
PIOS_DEBUG_Assert(0);
}
// Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * settings->VbarSensitivity[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0;
}
/**
* Want to keep the virtual flybar fixed in world coordinates as we pirouette
* @param[in] z_gyro The deg/s of rotation along the z axis
* @param[in] dT The time since last sample
*/
int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
{
const float F_PI = (float) M_PI;
float cy = cosf(z_gyro / 180.0f * F_PI * dT);
float sy = sinf(z_gyro / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
vbar_integral[1] = vbar_pitch;
vbar_integral[0] = vbar_roll;
return 0;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}

76
flight/Modules/Telemetry/telemetry.c
View File

@ -35,6 +35,10 @@
#include "flighttelemetrystats.h"
#include "gcstelemetrystats.h"
#include "hwsettings.h"
#if defined(PIOS_PACKET_HANDLER)
#include "pipxstatus.h"
#include "packet_handler.h"
#endif
// Private constants
#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE
@ -80,6 +84,10 @@ static void processObjEvent(UAVObjEvent * ev);
static void updateTelemetryStats();
static void gcsTelemetryStatsUpdated();
static void updateSettings();
static uint32_t getComPort();
#ifdef PIOS_PACKET_HANDLER
static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
#endif
/**
* Initialise the telemetry module
@ -93,6 +101,13 @@ int32_t TelemetryStart(void)
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
// Register to receive data from the radio packet handler.
// This must be after the radio module is initialized.
#ifdef PIOS_PACKET_HANDLER
if (PIOS_PACKET_HANDLER)
PHRegisterDataHandler(PIOS_PACKET_HANDLER, receivePacketData);
#endif
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
@ -247,6 +262,11 @@ static void processObjEvent(UAVObjEvent * ev)
retries = 0;
success = -1;
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
#ifdef PIOS_PACKET_HANDLER
// Don't send PipXStatus objects over the radio link.
if ((ev->obj == PipXStatusHandle()) && (getComPort() == 0) && PIOS_PACKET_HANDLER)
return;
#endif
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
@ -322,19 +342,10 @@ static void telemetryTxPriTask(void *parameters)
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
{
inputPort = telemetryPort;
}
uint32_t inputPort = getComPort();
if (inputPort) {
// Block until data are available
@ -362,23 +373,17 @@ static void telemetryRxTask(void *parameters)
*/
static int32_t transmitData(uint8_t * data, int32_t length)
{
uint32_t outputPort;
// Determine input port (USB takes priority over telemetry port)
#if defined(PIOS_INCLUDE_USB)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {
outputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
{
outputPort = telemetryPort;
}
uint32_t outputPort = getComPort();
if (outputPort) {
return PIOS_COM_SendBuffer(outputPort, data, length);
} else {
return -1;
}
#ifdef PIOS_PACKET_HANDLER
if (PIOS_PACKET_HANDLER)
if (PHTransmitData(PIOS_PACKET_HANDLER, data, length))
return length;
#endif
return -1;
}
/**
@ -564,6 +569,31 @@ static void updateSettings()
}
}
/**
* Determine input/output com port (USB takes priority over telemetry port)
*/
static uint32_t getComPort() {
#if defined(PIOS_INCLUDE_USB)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
return PIOS_COM_TELEM_USB;
else
#endif /* PIOS_INCLUDE_USB */
return telemetryPort;
}
#ifdef PIOS_PACKET_HANDLER
/**
* Receive a packet
* \param[in] buf The received data buffer
* \param[in] length Length of buffer
*/
static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
{
for (uint8_t i = 0; i < len; ++i)
UAVTalkProcessInputStream(uavTalkCon, buf[i]);
}
#endif
/**
* @}
* @}

3
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -238,6 +238,9 @@ static void vtolPathFollowerTask(void *parameters)
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
}

1
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -74,6 +74,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_WDG_STABILIZATION 0x0002
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_AUTOTUNE 0x0010
//------------------------
// TELEMETRY

38
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
View File

@ -138,8 +138,6 @@ extern uint32_t pios_i2c_flexi_adapter_id;
// See also pios_board.c
//------------------------
#define PIOS_SPI_MAX_DEVS 1
extern uint32_t pios_spi_port_id;
#define PIOS_SPI_PORT (pios_spi_port_id)
//-------------------------
// PIOS_USART
@ -270,30 +268,40 @@ extern uint32_t pios_ppm_rcvr_id;
// RFM22
//-------------------------
extern uint32_t pios_rfm22b_id;
#if defined(PIOS_INCLUDE_RFM22B)
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256
extern uint32_t pios_com_rfm22b_id;
#define PIOS_COM_RADIO (pios_com_rfm22b_id)
extern uint32_t pios_spi_rfm22b_id;
#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id)
#define RFM22_EXT_INT_USE
#define RFM22_PIOS_SPI PIOS_SPI_PORT // SPIx
extern uint32_t pios_rfm22b_id;
#endif /* PIOS_INCLUDE_RFM22B */
#if defined(RFM22_EXT_INT_USE)
#define PIOS_RFM22_EXTI_GPIO_PORT GPIOA
#define PIOS_RFM22_EXTI_GPIO_PIN GPIO_Pin_2
#define PIOS_RFM22_EXTI_PORT_SOURCE GPIO_PortSourceGPIOA
#define PIOS_RFM22_EXTI_PIN_SOURCE GPIO_PinSource2
#define PIOS_RFM22_EXTI_CLK RCC_APB2Periph_GPIOA
#define PIOS_RFM22_EXTI_LINE EXTI_Line2
#define PIOS_RFM22_EXTI_IRQn EXTI2_IRQn
#define PIOS_RFM22_EXTI_PRIO PIOS_IRQ_PRIO_LOW
#endif
//-------------------------
// Packet Handler
//-------------------------
#if defined(PIOS_INCLUDE_PACKET_HANDLER)
extern uint32_t pios_packet_handler;
#define PIOS_PACKET_HANDLER (pios_packet_handler)
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1
#define RS_ECC_NPARITY 4
#endif /* PIOS_INCLUDE_PACKET_HANDLER */
//-------------------------
// Packet Handler
//-------------------------
#if defined(PIOS_INCLUDE_PACKET_HANDLER)
uint32_t pios_packet_handler;
#define PIOS_INCLUDE_PACKET_HANDLER
#define PIOS_PACKET_HANDLER (pios_packet_handler)
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1
#endif /* PIOS_INCLUDE_PACKET_HANDLER */
//-------------------------
// Reed-Solomon ECC

290
flight/PiOS/Boards/STM32F4xx_RevoMini.h Normal file
View File

@ -0,0 +1,290 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file pios_board.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef STM3210E_INS_H_
#include <stdbool.h>
#define DEBUG_LEVEL 0
#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_aux_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_aux_id, __VA_ARGS__); }}
//------------------------
// Timers and Channels Used
//------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
//------------------------
// DMA Channels Used
//------------------------
/* Channel 1 - */
/* Channel 2 - SPI1 RX */
/* Channel 3 - SPI1 TX */
/* Channel 4 - SPI2 RX */
/* Channel 5 - SPI2 TX */
/* Channel 6 - */
/* Channel 7 - */
/* Channel 8 - */
/* Channel 9 - */
/* Channel 10 - */
/* Channel 11 - */
/* Channel 12 - */
//------------------------
// BOOTLOADER_SETTINGS
//------------------------
#define BOARD_READABLE true
#define BOARD_WRITABLE true
#define MAX_DEL_RETRYS 3
//------------------------
// PIOS_LED
//------------------------
#define PIOS_LED_HEARTBEAT 0
#define PIOS_LED_ALARM 1
//------------------------
// PIOS_SPI
// See also pios_board.c
//------------------------
#define PIOS_SPI_MAX_DEVS 3
//------------------------
// PIOS_WDG
//------------------------
#define PIOS_WATCHDOG_TIMEOUT 250
#define PIOS_WDG_REGISTER RTC_BKP_DR4
#define PIOS_WDG_ACTUATOR 0x0001
#define PIOS_WDG_STABILIZATION 0x0002
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_SENSORS 0x0010
//------------------------
// PIOS_I2C
// See also pios_board.c
//------------------------
#define PIOS_I2C_MAX_DEVS 3
extern uint32_t pios_i2c_mag_pressure_adapter_id;
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id)
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
//-------------------------
// PIOS_USART
//
// See also pios_board.c
//-------------------------
#define PIOS_USART_MAX_DEVS 5
//-------------------------
// PIOS_COM
//
// See also pios_board.c
//-------------------------
#define PIOS_COM_MAX_DEVS 4
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
extern uint32_t pios_com_vcp_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX
#if defined(PIOS_INCLUDE_RFM22B)
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
extern uint32_t pios_com_rfm22b_id;
#define PIOS_COM_RADIO (pios_com_rfm22b_id)
extern uint32_t pios_spi_telem_flash_id;
#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id)
#endif /* PIOS_INCLUDE_RFM22B */
//-------------------------
// Packet Handler
//-------------------------
#define RS_ECC_NPARITY 4
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1
extern uint32_t pios_packet_handler;
#define PIOS_PACKET_HANDLER (pios_packet_handler)
//------------------------
// TELEMETRY
//------------------------
#define TELEM_QUEUE_SIZE 80
#define PIOS_TELEM_STACK_SIZE 624
//-------------------------
// System Settings
//
// See also System_stm32f4xx.c
//-------------------------
//These macros are deprecated
//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below
//#define PIOS_MASTER_CLOCK
//#define PIOS_PERIPHERAL_CLOCK
//#define PIOS_PERIPHERAL_CLOCK
#define PIOS_SYSCLK 168000000
// Peripherals that belongs to APB1 are:
// DAC |PWR |CAN1,2
// I2C1,2,3 |UART4,5 |USART3,2
// I2S3Ext |SPI3/I2S3 |SPI2/I2S2
// I2S2Ext |IWDG |WWDG
// RTC/BKP reg
// TIM2,3,4,5,6,7,12,13,14
// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2)
// Default APB1 Prescaler = 4
#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2)
// Peripherals belonging to APB2
// SDIO |EXTI |SYSCFG |SPI1
// ADC1,2,3
// USART1,6
// TIM1,8,9,10,11
//
// Default APB2 Prescaler = 2
//
#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK
//-------------------------
// Interrupt Priorities
//-------------------------
#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_GCSRCVR_TIMEOUT_MS 100
//-------------------------
// Receiver PPM input
//-------------------------
#define PIOS_PPM_MAX_DEVS 1
#define PIOS_PPM_NUM_INPUTS 12
//-------------------------
// Receiver PWM input
//-------------------------
#define PIOS_PWM_MAX_DEVS 1
#define PIOS_PWM_NUM_INPUTS 8
//-------------------------
// Receiver SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_MAX_DEVS 2
#define PIOS_SPEKTRUM_NUM_INPUTS 12
//-------------------------
// Receiver S.Bus input
//-------------------------
#define PIOS_SBUS_MAX_DEVS 1
#define PIOS_SBUS_NUM_INPUTS (16+2)
//-------------------------
// Receiver DSM input
//-------------------------
#define PIOS_DSM_MAX_DEVS 2
#define PIOS_DSM_NUM_INPUTS 12
//-------------------------
// Servo outputs
//-------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
//--------------------------
// Timer controller settings
//--------------------------
#define PIOS_TIM_MAX_DEVS 6
//-------------------------
// ADC
// PIOS_ADC_PinGet(0) = Current sensor
// PIOS_ADC_PinGet(1) = Voltage sensor
// PIOS_ADC_PinGet(4) = VREF
// PIOS_ADC_PinGet(5) = Temperature sensor
//-------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \
{GPIOC, GPIO_Pin_2, ADC_Channel_12}, \
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \
{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \
{GPIOC, GPIO_Pin_2, ADC_Channel_12} \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
/* which is annoying because this then determines the rate at which we generate buffer turnover events */
/* the objective here is to get enough buffer space to support 100Hz averaging rate */
#define PIOS_ADC_NUM_CHANNELS 4
#define PIOS_ADC_MAX_OVERSAMPLING 2
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
//-------------------------
// USB
//-------------------------
#define PIOS_USB_MAX_DEVS 1
#define PIOS_USB_ENABLED 1 /* Should remove all references to this */
#define PIOS_USB_HID_MAX_DEVS 1
#endif /* STM3210E_INS_H_ */
/**
* @}
* @}
*/

2
flight/PiOS/Boards/pios_board.h
View File

@ -13,6 +13,8 @@
#include "STM32F2xx_INS.h"
#elif USE_STM32F4xx_OP
#include "STM32F4xx_Revolution.h"
#elif USE_STM32F4xx_RM
#include "STM32F4xx_RevoMini.h"
#else
#error Board definition has not been provided.
#endif

2
flight/PiOS/Common/pios_flash_jedec.c
View File

@ -196,6 +196,7 @@ int32_t PIOS_Flash_Jedec_Init(uint32_t spi_id, uint32_t slave_num, const struct
*/
int32_t PIOS_Flash_Jedec_StartTransaction()
{
return 0;
#if defined(FLASH_FREERTOS)
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
@ -212,6 +213,7 @@ int32_t PIOS_Flash_Jedec_StartTransaction()
*/
int32_t PIOS_Flash_Jedec_EndTransaction()
{
return 0;
#if defined(FLASH_FREERTOS)
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;

61
flight/PiOS/Common/pios_mpu6000.c
View File

@ -276,6 +276,7 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
*/
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data)
{
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
@ -451,19 +452,63 @@ void PIOS_MPU6000_IRQHandler(void)
PIOS_MPU6000_ReleaseBus();
}
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
// to our convention
#if defined(PIOS_MPU6000_ACCEL)
data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.accel_z = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
// Currently we only support rotations on top so switch X/Y accordingly
switch(dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
break;
case PIOS_MPU6000_TOP_90DEG:
data.accel_y = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.gyro_y = -(mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.accel_y = -(mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.accel_x = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.gyro_y = -(mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
data.gyro_x = -(mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
break;
case PIOS_MPU6000_TOP_270DEG:
data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.accel_x = -(mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
data.gyro_x = -(mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
break;
}
data.gyro_z = -(mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]);
data.accel_z = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10];
data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12];
data.gyro_z = mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14];
#else
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
data.gyro_z = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
switch(dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
break;
case PIOS_MPU6000_TOP_90DEG:
data.gyro_y = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.gyro_y = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]);
data.gyro_x = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
break;
case PIOS_MPU6000_TOP_270DEG:
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y
data.gyro_x = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X
break;
}
data.gyro_z = -(mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]);
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif
xQueueSend(dev->queue, (void *) &data, 0);

823
flight/PiOS/Common/pios_rfm22b.c
View File

@ -51,13 +51,8 @@
#if defined(PIOS_INCLUDE_RFM22B)
#include <string.h> // memmove
#include <stm32f10x.h>
#include <stopwatch.h>
#include <pios_spi_priv.h>
#include <packet_handler.h>
#include <pios_rfm22b_priv.h>
/* Local Defines */
@ -79,8 +74,6 @@
#define TX_TEST_MODE_TIMELIMIT_MS 30000 // TX test modes time limit (in ms)
//#define TX_PREAMBLE_NIBBLES 8 // 7 to 511 (number of nibbles)
//#define RX_PREAMBLE_NIBBLES 5 // 5 to 31 (number of nibbles)
#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles)
#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles)
@ -135,6 +128,17 @@
#define RFM22_DEFAULT_SS_RF_DATARATE 125 // 128bps
#ifndef RX_LED_ON
#define RX_LED_ON
#define RX_LED_OFF
#define TX_LED_ON
#define TX_LED_OFF
#define LINK_LED_ON
#define LINK_LED_OFF
#define USB_LED_ON
#define USB_LED_OFF
#endif
// ************************************
// Normal data streaming
// GFSK modulation
@ -154,8 +158,14 @@ struct pios_rfm22b_dev {
enum pios_rfm22b_dev_magic magic;
struct pios_rfm22b_cfg cfg;
uint32_t spi_id;
uint32_t slave_num;
uint32_t deviceID;
// ISR pending
xSemaphoreHandle isrPending;
// The COM callback functions.
pios_com_callback rx_in_cb;
uint32_t rx_in_context;
@ -169,27 +179,39 @@ struct pios_rfm22b_dev {
uint32_t random32 = 0x459ab8d8;
// Must ensure these prefilled arrays match the define sizes
static const uint8_t FULL_PREAMBLE[FIFO_SIZE] =
{PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,
PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes
static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1)/2 + 2] = {PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2};
static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
/* Local function forwared declarations */
static void PIOS_RFM22B_Supervisor(uint32_t ppm_id);
static void rfm22_processInt(void);
static void PIOS_RFM22_EXT_Int(void);
static void rfm22_setTxMode(uint8_t mode);
// SPI read/write functions
void rfm22_startBurstWrite(uint8_t addr);
inline void rfm22_burstWrite(uint8_t data)
{
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data);
}
void rfm22_endBurstWrite(void);
void rfm22_write(uint8_t addr, uint8_t data);
void rfm22_startBurstRead(uint8_t addr);
inline uint8_t rfm22_burstRead(void)
{
return PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff);
}
void rfm22_endBurstRead(void);
uint8_t rfm22_read(uint8_t addr);
static void rfm22_write(uint8_t addr, uint8_t data);
static uint8_t rfm22_read(uint8_t addr);
uint8_t rfm22_txStart();
/* Provide a COM driver */
@ -208,35 +230,6 @@ const struct pios_com_driver pios_rfm22b_com_driver = {
.bind_rx_cb = PIOS_RFM22B_RegisterRxCallback,
};
// External interrupt configuration
static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = {
.vector = PIOS_RFM22_EXT_Int,
.line = PIOS_RFM22_EXTI_LINE,
.pin = {
.gpio = PIOS_RFM22_EXTI_GPIO_PORT,
.init = {
.GPIO_Pin = PIOS_RFM22_EXTI_GPIO_PIN,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.irq = {
.init = {
.NVIC_IRQChannel = PIOS_RFM22_EXTI_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_RFM22_EXTI_PRIO,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = PIOS_RFM22_EXTI_LINE,
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Falling,
.EXTI_LineCmd = ENABLE,
},
},
};
// xtal 10 ppm, 434MHz
#define LOOKUP_SIZE 14
const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 64000, 128000, 192000, 256000};
@ -301,7 +294,7 @@ const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2
volatile bool initialized = false;
#if defined(RFM22_EXT_INT_USE)
volatile bool exec_using_spi; // set this if you want to access the SPI bus outside of the interrupt
volatile bool exec_using_spi; // set this if you want to access the SPI bus outside of the interrupt
#endif
uint8_t device_type; // the RF chips device ID number
@ -337,17 +330,6 @@ volatile int32_t afc_correction_Hz; // afc correction reading (in Hz)
volatile int16_t temperature_reg; // the temperature sensor reading
#if defined(RFM22_DEBUG)
volatile uint8_t prev_device_status; // just for debugging
volatile uint8_t prev_int_status1; // " "
volatile uint8_t prev_int_status2; // " "
volatile uint8_t prev_ezmac_status; // " "
const char *debug_msg = "";
const char *error_msg = "";
static uint32_t debug_val = 0;
#endif
volatile uint8_t osc_load_cap; // xtal frequency calibration value
volatile uint8_t rssi; // the current RSSI (register value)
@ -400,7 +382,7 @@ struct pios_rfm22b_dev * rfm22b_dev_g;
static bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev)
{
return (rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC);
return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS)
@ -409,6 +391,7 @@ static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void)
struct pios_rfm22b_dev * rfm22b_dev;
rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev));
rfm22b_dev->spi_id = 0;
if (!rfm22b_dev) return(NULL);
rfm22b_dev_g = rfm22b_dev;
@ -432,10 +415,12 @@ static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void)
}
#endif
static struct pios_rfm22b_dev * g_rfm22b_dev = NULL;
/**
* Initialise an RFM22B device
*/
int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg)
{
PIOS_DEBUG_Assert(rfm22b_id);
PIOS_DEBUG_Assert(cfg);
@ -445,10 +430,18 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
if (!rfm22b_dev)
return(-1);
// Store the SPI handle
rfm22b_dev->slave_num = slave_num;
rfm22b_dev->spi_id = spi_id;
// Bind the configuration to the device instance
rfm22b_dev->cfg = *cfg;
*rfm22b_id = (uint32_t)rfm22b_dev;
g_rfm22b_dev = rfm22b_dev;
// Create a semaphore to know if an ISR needs responding to
vSemaphoreCreateBinary( rfm22b_dev->isrPending );
// Initialize the TX pre-buffer pointer.
tx_pre_buffer_size = 0;
@ -469,6 +462,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
rfm22b_dev->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
rfm22b_dev->resets = 0;
// Initialize the external interrupt.
PIOS_EXTI_Init(cfg->exti_cfg);
// Initialize the radio device.
int initval = rfm22_init_normal(rfm22b_dev->deviceID, cfg->minFrequencyHz, cfg->maxFrequencyHz, 50000);
@ -504,7 +500,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
rfm22_setFreqCalibration(cfg->RFXtalCap);
rfm22_setNominalCarrierFrequency(cfg->frequencyHz);
rfm22_setDatarate(cfg->maxRFBandwidth, TRUE);
rfm22_setDatarate(cfg->maxRFBandwidth, true);
rfm22_setTxPower(cfg->maxTxPower);
DEBUG_PRINTF(2, "\n\r");
@ -666,125 +662,140 @@ static void PIOS_RFM22B_Supervisor(uint32_t rfm22b_id)
}
}
static void rfm22_setDebug(const char* msg)
{
debug_msg = msg;
}
static void rfm22_setError(const char* msg)
{
error_msg = msg;
}
// ************************************
// SPI read/write
void rfm22_startBurstWrite(uint8_t addr)
//! Assert the CS line
static void rfm22_assertCs()
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr);
if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0)
PIOS_SPI_RC_PinSet(g_rfm22b_dev->spi_id, g_rfm22b_dev->slave_num, 0);
}
void rfm22_endBurstWrite(void)
//! Deassert the CS line
static void rfm22_deassertCs()
{
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0)
PIOS_SPI_RC_PinSet(g_rfm22b_dev->spi_id, g_rfm22b_dev->slave_num, 1);
}
void rfm22_write(uint8_t addr, uint8_t data)
//! Claim the SPI bus semaphore
static void rfm22_claimBus()
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data);
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0)
PIOS_SPI_ClaimBus(g_rfm22b_dev->spi_id);
}
void rfm22_startBurstRead(uint8_t addr)
//! Release the SPI bus semaphore
static void rfm22_releaseBus()
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f);
if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0)
PIOS_SPI_ReleaseBus(g_rfm22b_dev->spi_id);
}
void rfm22_endBurstRead(void)
/**
* Claim the semaphore and write a byte to a register
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static void rfm22_write(uint8_t addr, uint8_t data)
{
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
if(PIOS_RFM22B_validate(g_rfm22b_dev)) {
rfm22_claimBus();
rfm22_assertCs();
uint8_t buf[2] = {addr | 0x80, data};
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
rfm22_deassertCs();
rfm22_releaseBus();
}
}
uint8_t rfm22_read(uint8_t addr)
/**
* Write a byte to a register without claiming the bus. Also
* toggle the NSS line
* @param[in] addr The address of the RFM22b register to write
* @param[in] data The data to write to that register
*/
static void rfm22_write_noclaim(uint8_t addr, uint8_t data)
{
uint8_t rdata;
uint8_t buf[2] = {addr | 0x80, data};
if(PIOS_RFM22B_validate(g_rfm22b_dev)) {
rfm22_assertCs();
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
rfm22_deassertCs();
}
}
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
/**
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
* Read a byte from an RFM22b register
* @param[in] addr The address to read from
* @return Returns the result of the register read
*/
static uint8_t rfm22_read(uint8_t addr)
{
uint8_t in[2];
uint8_t out[2] = {addr & 0x7f, 0xFF};
if(PIOS_RFM22B_validate(g_rfm22b_dev)) {
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
rfm22_deassertCs();
rfm22_releaseBus();
}
return in[1];
}
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f);
rdata = PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff);
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
return rdata;
/**
* Read a byte from an RFM22b register without claiming the bus
* @param[in] addr The address to read from
* @return Returns the result of the register read
*/
static uint8_t rfm22_read_noclaim(uint8_t addr)
{
uint8_t out[2] = {addr & 0x7F, 0xFF};
uint8_t in[2];
if (PIOS_RFM22B_validate(rfm22b_dev_g)) {
rfm22_assertCs();
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
rfm22_deassertCs();
}
return in[1];
}
// ************************************
// external interrupt
uint32_t rfm32_errors;
uint32_t rfm32_irqs_processed;
static void PIOS_RFM22_EXT_Int(void)
void PIOS_RFM22_EXT_Int(void)
{
rfm22_setDebug("Ext Int");
if (!exec_using_spi)
bool valid = PIOS_RFM22B_validate(g_rfm22b_dev);
PIOS_Assert(valid);
portBASE_TYPE pxHigherPriorityTaskWoken;
if (!exec_using_spi) {
if (xSemaphoreGiveFromISR(g_rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) {
// Something went fairly seriously wrong
rfm32_errors++;
}
portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken);
}
}
void PIOS_RFM22_processPendingISR(uint32_t wait_ms)
{
bool valid = PIOS_RFM22B_validate(g_rfm22b_dev);
PIOS_Assert(valid);
if ( xSemaphoreTake(g_rfm22b_dev->isrPending, wait_ms / portTICK_RATE_MS) == pdTRUE ) {
rfm32_irqs_processed++;
rfm22_processInt();
rfm22_setDebug("Ext Done");
}
}
void rfm22_disableExtInt(void)
{
#if defined(RFM22_EXT_INT_USE)
rfm22_setDebug("Disable Int");
// Configure the external interrupt
GPIO_EXTILineConfig(PIOS_RFM22_EXTI_PORT_SOURCE, PIOS_RFM22_EXTI_PIN_SOURCE);
EXTI_InitTypeDef EXTI_InitStructure = pios_exti_rfm22b_cfg.exti.init;
EXTI_InitStructure.EXTI_LineCmd = DISABLE;
EXTI_Init(&EXTI_InitStructure);
EXTI_ClearFlag(PIOS_RFM22_EXTI_LINE);
rfm22_setDebug("Disable Int done");
#endif
}
void rfm22_enableExtInt(void)
{
#if defined(RFM22_EXT_INT_USE)
rfm22_setDebug("Ensable Int");
if (PIOS_EXTI_Init(&pios_exti_rfm22b_cfg))
PIOS_Assert(0);
rfm22_setDebug("Ensable Int done");
#endif
}
// ************************************
// set/get the current tx power setting
@ -823,7 +834,7 @@ uint32_t rfm22_maxFrequency(void)
void rfm22_setNominalCarrierFrequency(uint32_t frequency_hz)
{
exec_using_spi = TRUE;
exec_using_spi = true;
// *******
@ -869,7 +880,7 @@ void rfm22_setNominalCarrierFrequency(uint32_t frequency_hz)
// DEBUG_PRINTF(2, "rf setFreq frequency_step_size: %0.2f\n\r", frequency_step_size);
#endif
exec_using_spi = FALSE;
exec_using_spi = false;
}
uint32_t rfm22_getNominalCarrierFrequency(void)
@ -914,7 +925,7 @@ uint32_t rfm22_freqHopSize(void)
void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening)
{
exec_using_spi = TRUE;
exec_using_spi = true;
lookup_index = 0;
while (lookup_index < (LOOKUP_SIZE - 1) && data_rate[lookup_index] < datarate_bps)
@ -1046,7 +1057,7 @@ void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening)
// *******
exec_using_spi = FALSE;
exec_using_spi = false;
}
uint32_t rfm22_getDatarate(void)
@ -1059,7 +1070,7 @@ uint32_t rfm22_getDatarate(void)
void rfm22_setSSBandwidth(uint32_t bandwidth_index)
{
exec_using_spi = TRUE;
exec_using_spi = true;
ss_lookup_index = bandwidth_index;
@ -1095,14 +1106,14 @@ void rfm22_setSSBandwidth(uint32_t bandwidth_index)
// *******
exec_using_spi = FALSE;
exec_using_spi = false;
}
// ************************************
void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode)
{
exec_using_spi = TRUE;
exec_using_spi = true;
// disable interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
@ -1151,41 +1162,21 @@ void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode)
// enable the receiver
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
exec_using_spi = FALSE;
}
// ************************************
uint16_t rfm22_addHeader()
{
uint16_t i = 0;
for (uint16_t j = (TX_PREAMBLE_NIBBLES + 1) / 2; j > 0; j--)
{
rfm22_burstWrite(PREAMBLE_BYTE);
i++;
}
rfm22_burstWrite(SYNC_BYTE_1); i++;
rfm22_burstWrite(SYNC_BYTE_2); i++;
return i;
exec_using_spi = false;
}
// ************************************
uint8_t rfm22_txStart()
{
if((tx_pre_buffer_size == 0) || (exec_using_spi == TRUE))
if((tx_pre_buffer_size == 0) || (exec_using_spi == true))
{
// Clear the TX buffer.
tx_data_rd = tx_data_wr = 0;
return 0;
}
exec_using_spi = TRUE;
// Disable interrrupts.
PIOS_IRQ_Disable();
exec_using_spi = true;
// Initialize the supervisor timer.
rfm22b_dev_g->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
@ -1232,10 +1223,15 @@ uint8_t rfm22_txStart()
rfm22_write(RFM22_transmit_packet_length, tx_data_wr);
// add some data
rfm22_startBurstWrite(RFM22_fifo_access);
for (uint16_t i = 0; (tx_data_rd < tx_data_wr) && (i < FIFO_SIZE); ++tx_data_rd, ++i)
rfm22_burstWrite(tx_buffer[tx_data_rd]);
rfm22_endBurstWrite();
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferByte(g_rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (tx_data_wr - tx_data_rd);
bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write;
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, &tx_buffer[tx_data_rd], NULL, bytes_to_write, NULL);
tx_data_rd += bytes_to_write;
rfm22_deassertCs();
rfm22_releaseBus();
// *******************
@ -1250,55 +1246,53 @@ uint8_t rfm22_txStart()
// enable the transmitter
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
// Re-ensable interrrupts.
PIOS_IRQ_Enable();
TX_LED_ON;
exec_using_spi = FALSE;
exec_using_spi = false;
return 1;
}
static void rfm22_setTxMode(uint8_t mode)
{
rfm22_setDebug("setTxMode");
if (mode != TX_DATA_MODE && mode != TX_STREAM_MODE && mode != TX_CARRIER_MODE && mode != TX_PN_MODE)
return; // invalid mode
exec_using_spi = TRUE;
rfm22_claimBus();
rfm22_assertCs();
// disable interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
rfm22_write(RFM22_interrupt_enable2, 0x00);
// Disaable interrupts (IE1, IE2 = 0)
uint8_t out_buf[3] = {RFM22_interrupt_enable1 | 0x80, 0x00, 0x00};
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, out_buf, NULL, sizeof(out_buf), NULL);
rfm22_deassertCs();
// TUNE mode
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
rfm22_write_noclaim(RFM22_op_and_func_ctrl1,RFM22_opfc1_pllon);
RX_LED_OFF;
// set the tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 |
// Set the tx power
rfm22_write_noclaim(RFM22_tx_power,RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 |
RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power);
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
uint8_t fd_bit = rfm22_read_noclaim(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
if (mode == TX_CARRIER_MODE)
// blank carrier mode - for testing
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
rfm22_write_noclaim(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
RFM22_mmc2_modtyp_none); // FIFO mode, Blank carrier
else if (mode == TX_PN_MODE)
// psuedo random data carrier mode - for testing
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
rfm22_write_noclaim(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
RFM22_mmc2_modtyp_gfsk); // FIFO mode, PN9 carrier
else
// data transmission
// FIFO mode, GFSK modulation
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
rfm22_write_noclaim(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
// clear FIFOs
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(RFM22_op_and_func_ctrl2, 0x00);
rfm22_write_noclaim(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write_noclaim(RFM22_op_and_func_ctrl2, 0x00);
// add some data to the chips TX FIFO before enabling the transmitter
{
@ -1307,30 +1301,26 @@ static void rfm22_setTxMode(uint8_t mode)
if (mode == TX_DATA_MODE)
// set the total number of data bytes we are going to transmit
rfm22_write(RFM22_transmit_packet_length, wr);
rfm22_write_noclaim(RFM22_transmit_packet_length, wr);
uint16_t max_bytes = FIFO_SIZE - 1;
uint16_t i = 0;
rfm22_startBurstWrite(RFM22_fifo_access);
rfm22_assertCs();
PIOS_SPI_TransferByte(g_rfm22b_dev->spi_id, 0x80 | RFM22_fifo_access); // Initiate burst write
if (mode == TX_STREAM_MODE) {
if (rd >= wr) {
// no data to send - yet .. just send preamble pattern
while (true) {
rfm22_burstWrite(PREAMBLE_BYTE);
if (++i >= max_bytes) break;
}
} else // add the RF heaader
i += rfm22_addHeader();
if (rd >= wr)
i += PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, FULL_PREAMBLE, NULL, sizeof(FULL_PREAMBLE), NULL);
else // add the RF heaader
i += PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, HEADER, NULL, sizeof(HEADER), NULL);
}
// add some data
for (uint16_t j = wr - rd; j > 0; j--) {
rfm22_burstWrite(tx_buffer[rd++]);
if (++i >= max_bytes)
break;
}
// Send data if there is any and there is space in the buffer available
// Bytes available to send minus how many we have sent
int32_t bytes_to_send = wr - rd;
bytes_to_send = ((bytes_to_send + i)> FIFO_SIZE) ? (FIFO_SIZE - i) : bytes_to_send;
if (bytes_to_send > 0)
rd += PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, &tx_buffer[rd], NULL, bytes_to_send, NULL);
rfm22_endBurstWrite();
rfm22_deassertCs();
tx_data_rd = rd;
}
@ -1342,23 +1332,18 @@ static void rfm22_setTxMode(uint8_t mode)
// enable TX interrupts
// rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem | RFM22_ie1_enfferr);
rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
rfm22_write_noclaim(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// read interrupt status - clear interrupts
rfm22_read(RFM22_interrupt_status1);
rfm22_read(RFM22_interrupt_status2);
rfm22_read_noclaim(RFM22_interrupt_status1);
rfm22_read_noclaim(RFM22_interrupt_status2);
// enable the transmitter
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton | RFM22_opfc1_txon);
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
rfm22_write_noclaim(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
rfm22_releaseBus();
TX_LED_ON;
// *******************
exec_using_spi = FALSE;
rfm22_setDebug("setTxMode end");
}
// ************************************
@ -1368,12 +1353,10 @@ void rfm22_processRxInt(void)
{
register uint8_t int_stat1 = int_status1;
register uint8_t int_stat2 = int_status2;
rfm22_setDebug("processRxInt");
// FIFO under/over flow error. Restart RX mode.
if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl))
{
rfm22_setError("R_UNDER/OVERRUN");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
@ -1383,7 +1366,6 @@ void rfm22_processRxInt(void)
{
rf_mode = RX_WAIT_SYNC_MODE;
RX_LED_ON;
rfm22_setDebug("pream_det");
}
// Sync word detected
@ -1391,7 +1373,6 @@ void rfm22_processRxInt(void)
{
rf_mode = RX_DATA_MODE;
RX_LED_ON;
rfm22_setDebug("sync_det");
// read the 10-bit signed afc correction value
// bits 9 to 2
@ -1420,7 +1401,6 @@ void rfm22_processRxInt(void)
// The received packet is going to be larger than the specified length
if ((rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len)
{
rfm22_setError("r_size_error1");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
@ -1428,23 +1408,27 @@ void rfm22_processRxInt(void)
// Another packet length error.
if (((rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(int_stat1 & RFM22_is1_ipkvalid))
{
rfm22_setError("r_size_error2");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// Fetch the data from the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
for (uint8_t i = 0; i < RX_FIFO_HI_WATERMARK; ++i)
rx_buffer[rx_buffer_wr++] = rfm22_burstRead();
rfm22_endBurstRead();
}
else
{ // just clear the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
for (register uint16_t i = RX_FIFO_HI_WATERMARK; i > 0; i--)
rfm22_burstRead(); // read a byte from the rf modules RX FIFO buffer
rfm22_endBurstRead();
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferByte(rfm22b_dev_g->spi_id,RFM22_fifo_access & 0x7F);
rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev_g->spi_id,OUT_FF,
(uint8_t *) &rx_buffer[rx_buffer_wr],RX_FIFO_HI_WATERMARK,NULL) == 0) ?
RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs();
rfm22_releaseBus();
} else {
// Clear the RX FIFO
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferByte(rfm22b_dev_g->spi_id,RFM22_fifo_access & 0x7F);
PIOS_SPI_TransferBlock(rfm22b_dev_g->spi_id,OUT_FF,NULL,RX_FIFO_HI_WATERMARK,NULL);
rfm22_deassertCs();
rfm22_releaseBus();
}
}
@ -1452,7 +1436,6 @@ void rfm22_processRxInt(void)
if (int_stat1 & RFM22_is1_icrerror)
{
rfm22_int_timer = 0; // reset the timer
rfm22_setError("CRC_ERR");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
@ -1462,29 +1445,31 @@ void rfm22_processRxInt(void)
{
// read the total length of the packet data
register uint16_t len = rfm22_read(RFM22_received_packet_length);
uint32_t len = rfm22_read(RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rx_buffer_wr < len)
{
int32_t bytes_to_read = len - rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
while (rx_buffer_wr < len)
rx_buffer[rx_buffer_wr++] = rfm22_burstRead();
rfm22_endBurstRead();
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferByte(rfm22b_dev_g->spi_id,RFM22_fifo_access & 0x7F);
rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev_g->spi_id,OUT_FF,
(uint8_t *) &rx_buffer[rx_buffer_wr],bytes_to_read,NULL) == 0) ?
bytes_to_read : 0;
rfm22_deassertCs();
rfm22_releaseBus();
}
if (rx_buffer_wr != len)
{
// we have a packet length error .. discard the packet
rfm22_setError("r_pack_len_error");
debug_val = len;
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// we have a valid received packet
rfm22_setDebug("VALID_R_PACKET");
if (rx_buffer_wr > 0)
{
@ -1507,12 +1492,10 @@ void rfm22_processRxInt(void)
if(!rfm22_txStart())
{
// Switch to RX mode
rfm22_setDebug(" Set RX");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
}
}
rfm22_setDebug("processRxInt end");
}
void rfm22_processTxInt(void)
@ -1525,7 +1508,6 @@ void rfm22_processTxInt(void)
// FIFO under/over flow error. Back to RX mode.
if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl))
{
rfm22_setError("T_UNDER/OVERRUN");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
@ -1544,7 +1526,6 @@ void rfm22_processTxInt(void)
if (rfm22_int_timer >= 100)
{
rfm22_int_time_outs++;
rfm22_setError("T_TIMEOUT");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // back to rx mode
return;
}
@ -1555,53 +1536,61 @@ void rfm22_processTxInt(void)
{
// top-up the rf chips TX FIFO buffer
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_startBurstWrite(RFM22_fifo_access);
for (uint16_t i = 0; (tx_data_rd < tx_data_wr) && (i < max_bytes); ++i, ++tx_data_rd)
rfm22_burstWrite(tx_buffer[tx_data_rd]);
rfm22_endBurstWrite();
rfm22_claimBus();
rfm22_assertCs();
PIOS_SPI_TransferByte(g_rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (tx_data_wr - tx_data_rd);
bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write;
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, &tx_buffer[tx_data_rd], NULL, bytes_to_write, NULL);
tx_data_rd += bytes_to_write;
rfm22_deassertCs();
rfm22_releaseBus();
}
// Packet has been sent
if (int_stat1 & RFM22_is1_ipksent)
{
rfm22_setDebug(" T_Sent");
// Send another packet if it's available.
if(!rfm22_txStart())
{
// Switch to RX mode
rfm22_setDebug(" Set RX");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
}
rfm22_setDebug("ProcessTxInt done");
}
static void rfm22_processInt(void)
{
rfm22_setDebug("ProcessInt");
// this is called from the external interrupt handler
if (!initialized || power_on_reset)
// we haven't yet been initialized
// we haven't yet been initialized
if (!initialized || power_on_reset || !PIOS_RFM22B_validate(rfm22b_dev_g))
return;
exec_using_spi = TRUE;
exec_using_spi = true;
// Reset the supervisor timer.
rfm22b_dev_g->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
// read interrupt status registers - clears the interrupt line
int_status1 = rfm22_read(RFM22_interrupt_status1);
int_status2 = rfm22_read(RFM22_interrupt_status2);
// 1. Read the interrupt statuses with burst read
rfm22_claimBus(); // Set RC and the semaphore
uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF};
uint8_t read_buf[3];
rfm22_assertCs();
PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL);
rfm22_deassertCs();
int_status1 = read_buf[1];
int_status2 = read_buf[2];
// Device status
device_status = rfm22_read_noclaim(RFM22_device_status);
// read device status register
device_status = rfm22_read(RFM22_device_status);
// EzMAC status
ezmac_status = rfm22_read_noclaim(RFM22_ezmac_status);
// read ezmac status register
ezmac_status = rfm22_read(RFM22_ezmac_status);
// Release the bus
rfm22_releaseBus();
// Read the RSSI if we're in RX mode
if (rf_mode != TX_DATA_MODE && rf_mode != TX_STREAM_MODE &&
@ -1619,9 +1608,8 @@ static void rfm22_processInt(void)
// the RF module has gone and done a reset - we need to re-initialize the rf module
if (int_status2 & RFM22_is2_ipor)
{
initialized = FALSE;
power_on_reset = TRUE;
rfm22_setError("Reset");
initialized = false;
power_on_reset = true;
// Need to do something here!
return;
}
@ -1661,21 +1649,19 @@ static void rfm22_processInt(void)
break;
}
exec_using_spi = FALSE;
rfm22_setDebug("ProcessInt done");
exec_using_spi = false;
}
// ************************************
int8_t rfm22_getRSSI(void)
{
exec_using_spi = TRUE;
exec_using_spi = true;
rssi = rfm22_read(RFM22_rssi); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm
rssi_dBm = (int8_t)(rssi >> 1) - 122; // convert to dBm
exec_using_spi = FALSE;
exec_using_spi = false;
return rssi_dBm;
}
@ -1757,32 +1743,32 @@ int8_t rfm22_currentMode(void)
return rf_mode;
}
// return TRUE if we are transmitting
// return true if we are transmitting
bool rfm22_transmitting(void)
{
return (rf_mode == TX_DATA_MODE || rf_mode == TX_STREAM_MODE || rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE);
}
// return TRUE if the channel is clear to transmit on
// return true if the channel is clear to transmit on
bool rfm22_channelIsClear(void)
{
if (!initialized)
// we haven't yet been initialized
return FALSE;
return false;
if (rf_mode != RX_WAIT_PREAMBLE_MODE && rf_mode != RX_WAIT_SYNC_MODE)
// we are receiving something or we are transmitting or we are scanning the spectrum
return FALSE;
return false;
return TRUE;
return true;
}
// return TRUE if the transmiter is ready for use
// return true if the transmiter is ready for use
bool rfm22_txReady(void)
{
if (!initialized)
// we haven't yet been initialized
return FALSE;
return false;
return (tx_data_rd == 0 && tx_data_wr == 0 && rf_mode != TX_DATA_MODE &&
rf_mode != TX_STREAM_MODE && rf_mode != TX_CARRIER_MODE && rf_mode != TX_PN_MODE &&
@ -1807,11 +1793,11 @@ void rfm22_setFreqCalibration(uint8_t value)
tx_data_rd = tx_data_wr = 0;
}
exec_using_spi = TRUE;
exec_using_spi = true;
rfm22_write(RFM22_xtal_osc_load_cap, osc_load_cap);
exec_using_spi = FALSE;
exec_using_spi = false;
if (prev_rf_mode == TX_CARRIER_MODE || prev_rf_mode == TX_PN_MODE)
rfm22_setTxMode(prev_rf_mode);
@ -1842,40 +1828,24 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq
{
initialized = false;
#if defined(RFM22_EXT_INT_USE)
rfm22_disableExtInt();
#endif
power_on_reset = false;
// ****************
exec_using_spi = TRUE;
// ****************
// setup the SPI port
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
// set SPI port SCLK frequency .. 4.5MHz
PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_16);
// set SPI port SCLK frequency .. 2.25MHz
// PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_32);
// set SPI port SCLK frequency .. 285kHz .. purely for hardware fault finding
// PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_256);
exec_using_spi = true;
// ****************
// software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); // software reset the radio
PIOS_DELAY_WaitmS(26); // wait 26ms
// wait 26ms
PIOS_DELAY_WaitmS(26);
for (int i = 50; i > 0; i--)
{
PIOS_DELAY_WaitmS(1); // wait 1ms
// wait 1ms
PIOS_DELAY_WaitmS(1);
// read the status registers
int_status1 = rfm22_read(RFM22_interrupt_status1);
@ -1897,7 +1867,7 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq
// ****************
exec_using_spi = FALSE;
exec_using_spi = false;
// ****************
@ -2010,8 +1980,13 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq
// choose the 3 GPIO pin functions
rfm22_write(RFM22_io_port_config, RFM22_io_port_default); // GPIO port use default value
rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); // GPIO0 = TX State (to control RF Switch)
rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); // GPIO1 = RX State (to control RF Switch)
if (rfm22b_dev_g->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) {
rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); // GPIO0 = TX State (to control RF Switch)
rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); // GPIO1 = RX State (to control RF Switch)
} else {
rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); // GPIO0 = TX State (to control RF Switch)
rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); // GPIO1 = RX State (to control RF Switch)
}
rfm22_write(RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // GPIO2 = Clear Channel Assessment
// ****************
@ -2019,197 +1994,6 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq
return 0; // OK
}
// ************************************
int rfm22_init_scan_spectrum(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init scan spectrum\n\r");
#endif
int res = rfm22_resetModule(RX_SCAN_SPECTRUM, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
// rfm22_setSSBandwidth(0);
rfm22_setSSBandwidth(1);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
rfm22_write(RFM22_cpu_output_clk, RFM22_coc_1MHz); // 1MHz clock output
rfm22_write(RFM22_rssi_threshold_clear_chan_indicator, 0);
rfm22_write(RFM22_preamble_detection_ctrl1, 31 << 3); // 31-nibbles rx preamble detection
// avoid packet detection
rfm22_write(RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_encrc);
rfm22_write(RFM22_header_control1, 0x0f);
rfm22_write(RFM22_header_control2, 0x77);
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1);
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2);
rfm22_write(RFM22_sync_word1, SYNC_BYTE_3 ^ 0xff);
rfm22_write(RFM22_sync_word0, SYNC_BYTE_4 ^ 0xff);
// all the bits to be checked
rfm22_write(RFM22_header_enable3, 0xff);
rfm22_write(RFM22_header_enable2, 0xff);
rfm22_write(RFM22_header_enable1, 0xff);
rfm22_write(RFM22_header_enable0, 0xff);
// set frequency hopping channel step size (multiples of 10kHz)
// rfm22_write(RFM22_frequency_hopping_step_size, 0);
// set our nominal carrier frequency
rfm22_setNominalCarrierFrequency(min_frequency_hz);
// set minimum tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | 0);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_SCAN_SPECTRUM, true);
initialized = true;
return 0; // OK
}
// ************************************
int rfm22_init_tx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init TX stream\n\r");
#endif
int res = rfm22_resetModule(TX_STREAM_MODE, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
frequency_hop_step_size_reg = 0;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
// disable the internal Tx & Rx packet handlers (without CRC)
rfm22_write(RFM22_data_access_control, 0);
rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble
rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection
rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header
rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length).
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); //
// rfm22_write(RFM22_modem_test, 0x01);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz)
rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power); // set the tx power
// rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | tx_power); // set the tx power
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
rfm22_write(RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); // TX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); // TX FIFO Almost Empty Threshold (0 - 63)
// Enable RF module external interrupt
rfm22_enableExtInt();
initialized = true;
return 0; // OK
}
// ************************************
int rfm22_init_rx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init RX stream\n\r");
#endif
int res = rfm22_resetModule(RX_WAIT_PREAMBLE_MODE, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
frequency_hop_step_size_reg = 0;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
// disable the internal Tx & Rx packet handlers (without CRC)
rfm22_write(RFM22_data_access_control, 0);
rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble
rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection
rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header
rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length).
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); //
// no header bits to be checked
rfm22_write(RFM22_header_enable3, 0x00);
rfm22_write(RFM22_header_enable2, 0x00);
rfm22_write(RFM22_header_enable1, 0x00);
rfm22_write(RFM22_header_enable0, 0x00);
// rfm22_write(RFM22_modem_test, 0x01);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz)
rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
// RX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
initialized = true;
return 0; // OK
}
// ************************************
// Initialise this hardware layer module and the rf module
@ -2225,7 +2009,7 @@ int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequ
frequency_hop_step_size_reg = freq_hop_step_size;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, TRUE);
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, true);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
@ -2322,9 +2106,6 @@ int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequ
// RX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
initialized = true;

84
flight/PiOS/Common/pios_usb_util.c
View File

@ -1,42 +1,42 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file pios_usb_util.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios_usb_util.h"
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen)
{
for (uint8_t i = 0; i < srclen; i++) {
*dst = *src;
dst += 2;
src += 1;
}
return dst;
}
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file pios_usb_util.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios_usb_util.h"
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen)
{
for (uint8_t i = 0; i < srclen; i++) {
*dst = *src;
dst += 2;
src += 1;
}
return dst;
}

1
flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld
View File

@ -75,6 +75,7 @@ SECTIONS
{
. = ALIGN(4);
KEEP(*(.boardinfo))
. = ALIGN(ORIGIN(BD_INFO)+LENGTH(BD_INFO));
} > BD_INFO
/* Stabs debugging sections. */

8
flight/PiOS/inc/pios_mpu6000.h
View File

@ -123,6 +123,13 @@ enum pios_mpu6000_accel_range {
PIOS_MPU6000_ACCEL_16G = 0x18
};
enum pios_mpu6000_orientation { // clockwise rotation from board forward
PIOS_MPU6000_TOP_0DEG = 0x00,
PIOS_MPU6000_TOP_90DEG = 0x01,
PIOS_MPU6000_TOP_180DEG = 0x02,
PIOS_MPU6000_TOP_270DEG = 0x03
};
struct pios_mpu6000_data {
int16_t gyro_x;
int16_t gyro_y;
@ -147,6 +154,7 @@ struct pios_mpu6000_cfg {
enum pios_mpu6000_accel_range accel_range;
enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_filter filter;
enum pios_mpu6000_orientation orientation;
};
/* Public Functions */

9
flight/PiOS/inc/pios_rfm22b.h
View File

@ -31,21 +31,28 @@
#ifndef PIOS_RFM22B_H
#define PIOS_RFM22B_H
enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX};
/* Global Types */
struct pios_rfm22b_cfg {
const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */
const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */
uint32_t frequencyHz;
uint32_t minFrequencyHz;
uint32_t maxFrequencyHz;
uint8_t RFXtalCap;
uint32_t maxRFBandwidth;
uint8_t maxTxPower;
uint8_t slave_num;
enum gpio_direction gpio_direction;
};
/* Public Functions */
extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, const struct pios_rfm22b_cfg *cfg);
extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg);
extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
extern int8_t PIOS_RFM22B_RSSI(uint32_t rfm22b_id);
extern int16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id);
extern void PIOS_RFM22_processPendingISR(uint32_t wait_ms);
#endif /* PIOS_RFM22B_H */

2
flight/PiOS/inc/pios_rfm22b_priv.h
View File

@ -591,6 +591,8 @@ typedef bool ( *t_rfm22_RxDataCallback ) (void *data, uint8_t len);
// ************************************
void PIOS_RFM22_EXT_Int(void);
uint32_t rfm22_minFrequency(void);
uint32_t rfm22_maxFrequency(void);

1
flight/PiOS/inc/pios_sbus_priv.h
View File

@ -79,6 +79,7 @@ struct pios_sbus_cfg {
void (*gpio_clk_func)(uint32_t periph, FunctionalState state);
uint32_t gpio_clk_periph;
BitAction gpio_inv_enable;
BitAction gpio_inv_disable;
};
extern const struct pios_rcvr_driver pios_sbus_rcvr_driver;

1
flight/PiOS/inc/pios_usb_defs.h
View File

@ -364,6 +364,7 @@ enum usb_op_board_ids {
USB_OP_BOARD_ID_PIPXTREME = 3,
USB_OP_BOARD_ID_COPTERCONTROL = 4,
USB_OP_BOARD_ID_REVOLUTION = 5,
USB_OP_BOARD_ID_OSD = 6,
} __attribute__((packed));
enum usb_op_board_modes {

76
flight/PiOS/inc/pios_usb_util.h
View File

@ -1,38 +1,38 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file pios_usb_util.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_UTIL_H
#define PIOS_USB_UTIL_H
#include <stdint.h> /* uint8_t */
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen);
#endif /* PIOS_USB_UTIL_H */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file pios_usb_util.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_UTIL_H
#define PIOS_USB_UTIL_H
#include <stdint.h> /* uint8_t */
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen);
#endif /* PIOS_USB_UTIL_H */

2
flight/PipXtreme/Makefile
View File

@ -66,7 +66,7 @@ FLASH_TOOL = OPENOCD
# List of modules to include
OPTMODULES =
MODULES = RadioComBridge
MODULES = Radio RadioComBridge
# Paths
OPSYSTEM = ./System

5
flight/PipXtreme/System/inc/pios_config.h
View File

@ -61,6 +61,8 @@
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_FLASH_EEPROM
#define PIOS_INCLUDE_RFM22B
#define PIOS_INCLUDE_PACKET_HANDLER
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
@ -96,6 +98,9 @@
/* PIOS Initcall infrastructure */
#define PIOS_INCLUDE_INITCALL
/* Always include the radio module */
#define RADIO_BUILTIN
#endif /* PIOS_CONFIG_H */
/**
* @}

119
flight/PipXtreme/System/pios_board.c
View File

@ -33,20 +33,17 @@
#include <pipxsettings.h>
#include <board_hw_defs.c>
#define PIOS_COM_SERIAL_RX_BUF_LEN 128
#define PIOS_COM_SERIAL_TX_BUF_LEN 128
#define PIOS_COM_SERIAL_RX_BUF_LEN 256
#define PIOS_COM_SERIAL_TX_BUF_LEN 256
#define PIOS_COM_FLEXI_RX_BUF_LEN 128
#define PIOS_COM_FLEXI_RX_BUF_LEN 256
#define PIOS_COM_FLEXI_TX_BUF_LEN 128
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256
#define PIOS_COM_VCP_USB_RX_BUF_LEN 128
#define PIOS_COM_VCP_USB_TX_BUF_LEN 128
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128
#define PIOS_COM_VCP_USB_RX_BUF_LEN 256
#define PIOS_COM_VCP_USB_TX_BUF_LEN 256
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telemetry_id;
@ -56,8 +53,6 @@ uint32_t pios_com_uavtalk_com_id = 0;
uint32_t pios_com_gcs_com_id = 0;
uint32_t pios_com_trans_com_id = 0;
uint32_t pios_com_debug_id = 0;
uint32_t pios_com_rfm22b_id = 0;
uint32_t pios_rfm22b_id = 0;
uint32_t pios_ppm_rcvr_id = 0;
/**
@ -70,14 +65,14 @@ void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Set up the SPI interface to the serial flash */
if (PIOS_SPI_Init(&pios_spi_port_id, &pios_spi_port_cfg)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
/* Set up the SPI interface to the rfm22b */
if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) {
PIOS_DEBUG_Assert(0);
}
#ifdef PIOS_INCLUDE_WDG
/* Initialize watchdog as early as possible to catch faults during init */
@ -120,10 +115,6 @@ void PIOS_Board_Init(void) {
PIOS_TIM_InitClock(&tim_4_cfg);
#endif /* PIOS_INCLUDE_TIM */
#if defined(PIOS_INCLUDE_PACKET_HANDLER)
pios_packet_handler = PHInitialize(&pios_ph_cfg);
#endif /* PIOS_INCLUDE_PACKET_HANDLER */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
@ -188,10 +179,6 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_USB_HID)
if (!usb_hid_present) {
PIOS_Assert(0);
}
/* Configure the usb HID port */
#if defined(PIOS_INCLUDE_COM)
{
@ -313,88 +300,6 @@ void PIOS_Board_Init(void) {
break;
}
#if defined(PIOS_INCLUDE_RFM22B)
struct pios_rfm22b_cfg pios_rfm22b_cfg = {
.frequencyHz = 434000000,
.minFrequencyHz = 434000000 - 2000000,
.maxFrequencyHz = 434000000 + 2000000,
.RFXtalCap = 0x7f,
.maxRFBandwidth = 128000,
.maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW
};
/* Retrieve hardware settings. */
pios_rfm22b_cfg.frequencyHz = pipxSettings.Frequency;
pios_rfm22b_cfg.RFXtalCap = pipxSettings.FrequencyCalibration;
switch (pipxSettings.RFSpeed)
{
case PIPXSETTINGS_RFSPEED_2400:
pios_rfm22b_cfg.maxRFBandwidth = 2000;
break;
case PIPXSETTINGS_RFSPEED_4800:
pios_rfm22b_cfg.maxRFBandwidth = 4000;
break;
case PIPXSETTINGS_RFSPEED_9600:
pios_rfm22b_cfg.maxRFBandwidth = 9600;
break;
case PIPXSETTINGS_RFSPEED_19200:
pios_rfm22b_cfg.maxRFBandwidth = 19200;
break;
case PIPXSETTINGS_RFSPEED_38400:
pios_rfm22b_cfg.maxRFBandwidth = 32000;
break;
case PIPXSETTINGS_RFSPEED_57600:
pios_rfm22b_cfg.maxRFBandwidth = 64000;
break;
case PIPXSETTINGS_RFSPEED_115200:
pios_rfm22b_cfg.maxRFBandwidth = 128000;
break;
}
switch (pipxSettings.MaxRFPower)
{
case PIPXSETTINGS_MAXRFPOWER_125:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_0;
break;
case PIPXSETTINGS_MAXRFPOWER_16:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_1;
break;
case PIPXSETTINGS_MAXRFPOWER_316:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_2;
break;
case PIPXSETTINGS_MAXRFPOWER_63:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_3;
break;
case PIPXSETTINGS_MAXRFPOWER_126:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_4;
break;
case PIPXSETTINGS_MAXRFPOWER_25:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_5;
break;
case PIPXSETTINGS_MAXRFPOWER_50:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_6;
break;
case PIPXSETTINGS_MAXRFPOWER_100:
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_7;
break;
}
/* Initalize the RFM22B radio COM device. */
{
if (PIOS_RFM22B_Init(&pios_rfm22b_id, &pios_rfm22b_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_RFM22B */
/* Remap AFIO pin */
GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);

512
flight/RevoMini/Makefile Normal file
View File

@ -0,0 +1,512 @@
#####
# Project: OpenPilot RevoMini
#
#
# Makefile for OpenPilot RevoMini project
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2012.
#
#
# This program 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.
#
# This program 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 this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := fw_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# List of modules to include
MODULES = Sensors
MODULES += Attitude/revolution
MODULES += ManualControl Stabilization Actuator
MODULES += Battery
MODULES += Altitude/revolution
MODULES += GPS FirmwareIAP
#MODULES += Airspeed/revolution
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += CameraStab
MODULES += Radio
MODULES += Telemetry
PYMODULES =
#FlightPlan
# Paths
OPSYSTEM = ./System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ../UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = ../Libraries/inc
MATHLIB = ../Libraries/math
MATHLIBINC = ../Libraries/math
RSCODE = $(FLIGHTLIB)/rscode
RSCODEINC = $(FLIGHTLIB)/rscode
PIOSSTM32F4XX = $(PIOS)/STM32F4xx
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
APPLIBDIR = $(PIOSSTM32F4XX)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F4xx_StdPeriph_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
SRC =
# optional component libraries
include $(PIOSCOMMONLIB)/FreeRTOS/library.mk
#include $(PIOSCOMMONLIB)/dosfs/library.mk
include $(PIOSCOMMONLIB)/msheap/library.mk
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## PyMite files and modules
SRC += $(OUTDIR)/pmlib_img.c
SRC += $(OUTDIR)/pmlib_nat.c
SRC += $(OUTDIR)/pmlibusr_img.c
SRC += $(OUTDIR)/pmlibusr_nat.c
PYSRC += $(wildcard ${PYMITEVM}/*.c)
PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += $(PYSRC)
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/revolution.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/pios_usb_board_data.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
#ifeq ($(DEBUG),YES)
SRC += $(OPSYSTEM)/dcc_stdio.c
SRC += $(OPSYSTEM)/cm3_fault_handlers.c
#endif
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/paths.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
## For RFM22b
SRC += $(FLIGHTLIB)/packet_handler.c
SRC += $(RSCODE)/berlekamp.c
SRC += $(RSCODE)/crcgen.c
SRC += $(RSCODE)/galois.c
SRC += $(RSCODE)/rs.c
## PIOS Hardware (STM32F4xx)
include $(PIOS)/STM32F4xx/library.mk
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_mpu6000.c
SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c
SRC += $(PIOSCOMMON)/pios_ms5611.c
SRC += $(PIOSCOMMON)/pios_crc.c
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_rfm22b.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/pios_flash_jedec.c
SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
SRC += $(PIOSCOMMON)/pios_usb_util.c
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
# List Assembler source files here which must be assembled in ARM-Mode..
ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(RSCODEINC)
EXTRAINCDIRS += $(PIOSSTM32F4XX)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(OPUAVSYNTHDIR)
EXTRAINCDIRS += $(BOOTINC)
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += $(HWDEFSINC)
# Generate intermediate code
gencode: ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h
$(PYSRC): gencode
PYTHON = python
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
@echo $(MSG_PYMITEINIT) $(call toprel, $@)
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
EXTRAINCDIRS += ${foreach MOD, ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F4XX)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
CFLAGS += -O0
CFLAGS += -DGENERAL_COV
CFLAGS += -finstrument-functions -ffixed-r10
else
CFLAGS += -Os
endif
# common architecture-specific flags from the device-specific library makefile
CFLAGS += $(ARCHFLAGS)
CFLAGS += -DDIAGNOSTICS
CFLAGS += -DDIAG_TASKS
# This is not the best place for these. Really should abstract out
# to the board file or something
CFLAGS += -DSTM32F4XX
CFLAGS += -DMEM_SIZE=1024000000
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
CFLAGS += -g$(DEBUGF)
CFLAGS += -ffast-math
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
#CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = $(ARCHFLAGS) -mthumb -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
#Linker scripts
LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_APP))
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
PYHON = python
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: gccversion build
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino opfw
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
opfw: $(OUTDIR)/$(TARGET).opfw
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).opfw
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

669
flight/RevoMini/Makefile.osx Normal file
View File

@ -0,0 +1,669 @@
#####
# Project: RevoMini
#
#
# Makefile for OpenPilot project build PiOS and the AP.
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2012.
#
#
# This program 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.
#
# This program 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 this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# Set developer code and compile options
# Set to YES to compile for debugging
DEBUG ?= YES
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
#
USE_BOOTLOADER ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
# Remove command is different for Code Sourcery on Windows
REMOVE_CMD ?= rm
FLASH_TOOL = OPENOCD
# YES enables -mthumb option to flags for source-files listed
# in SRC and CPPSRC
USE_THUMB_MODE = YES
# List of modules to include
MODULES += Actuator ManualControl Stabilization
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += Attitude/revolution
#MODULES += OveroSync/simulated
# To run simulation instead of connect to SITL
MODULES += Sensors/simulated
MODULES += Telemetry
# MCU name, submodel and board
# - MCU used for compiler-option (-mtune)
# - MODEL used for linker-script name (-T) and passed as define
# - BOARD just passed as define (optional)
MCU = i686
#CHIP = STM32F103RET
#BOARD = STM3210E_OP
MODEL = HD
ifeq ($(USE_BOOTLOADER), YES)
BOOT_MODEL = $(MODEL)_BL
else
BOOT_MODEL = $(MODEL)_NB
endif
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR = ../../build/sim_osx
# Target file name (without extension).
TARGET = RevoMini
# Paths
OPSYSTEM = ./System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ../UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = $(FLIGHTLIB)/inc
PIOS = ../PiOS.osx
PIOSINC = $(PIOS)/inc
PIOSPOSIX = $(PIOS)/osx
APPLIBDIR = $(PIOSPOSIX)/Libraries
RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
MODNAMES = $(notdir ${MODULES})
ifndef TESTAPP
## PyMite files
SRC += $(OUTDIR)/pmlib_img.c
SRC += $(OUTDIR)/pmlib_nat.c
SRC += $(OUTDIR)/pmlibusr_img.c
SRC += $(OUTDIR)/pmlibusr_nat.c
SRC += $(wildcard ${PYMITEVM}/*.c)
SRC += $(wildcard ${PYMITEPLAT}/*.c)
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/revolution.c
SRC += $(OPSYSTEM)/pios_board_sim.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
else
## TESTCODE
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
## UAVOBJECTS
ifndef TESTAPP
#include $(UAVOBJSYNTHDIR)/Makefile.inc
include ./UAVObjects.inc
UAVOBJSRCFILENAMES += attitudesimulated
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
SRC += $(UAVOBJSRC)
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
endif
## PIOS Hardware (posix)
SRC += $(PIOSPOSIX)/pios_crc.c
SRC += $(PIOSPOSIX)/pios_sys.c
SRC += $(PIOSPOSIX)/pios_led.c
SRC += $(PIOSPOSIX)/pios_irq.c
SRC += $(PIOSPOSIX)/pios_delay.c
SRC += $(PIOSPOSIX)/pios_sdcard.c
SRC += $(PIOSPOSIX)/pios_udp.c
SRC += $(PIOSPOSIX)/pios_tcp.c
SRC += $(PIOSPOSIX)/pios_com.c
SRC += $(PIOSPOSIX)/pios_servo.c
SRC += $(PIOSPOSIX)/pios_wdg.c
SRC += $(PIOSPOSIX)/pios_debug.c
SRC += $(PIOSPOSIX)/pios_rcvr.c
SRC += $(PIOSPOSIX)/pios_gcsrcvr.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/paths.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/taskmonitor.c
## RTOS and RTOS Portable
SRC += $(RTOSSRCDIR)/list.c
SRC += $(RTOSSRCDIR)/queue.c
UNAME := $(shell uname)
SRC += $(RTOSSRCDIR)/task.c
SRC += $(RTOSSRCDIR)/timers.c
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port.c
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSPOSIX)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Posix
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += ${foreach MOD, ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
#DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
ifeq ($(ENABLE_DEBUG_PINS), YES)
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
endif
ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
ifeq ($(USE_BOOTLOADER), YES)
CDEFS += -DUSE_BOOTLOADER
endif
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS = -g$(DEBUGF) -DDEBUG
endif
CFLAGS += -DDIAGNOSTICS
CFLAGS += -DDIAG_TASKS
CFLAGS += $(CFLAGS_UAVOBJECTS)
CFLAGS += -DARCH_POSIX
CFLAGS += -O$(OPT)
CFLAGS += -mtune=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
#CFLAGS += ARCH=arm
#CROSS_COMPILE=/usr/local/android-ndk-r5/toolchains/arm-linux-androideabi-4.4.3/prebuilt/darwin-x86/bin/arm-linux-androideabi-
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS += -lpthread
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# To include simulation model
LDFLAGS += -L$(OUTDIR)
#LDFLAGS += -lsimmodel
# Define programs and commands.
CC = $(TCHAIN_PREFIX)gcc
CPP = $(TCHAIN_PREFIX)g++
AR = $(TCHAIN_PREFIX)ar
OBJCOPY = $(TCHAIN_PREFIX)objcopy
OBJDUMP = $(TCHAIN_PREFIX)objdump
SIZE = $(TCHAIN_PREFIX)size
NM = $(TCHAIN_PREFIX)nm
REMOVE = $(REMOVE_CMD) -f
PYTHON = python
###SHELL = sh
###COPY = cp
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
MSG_END = ${quote}-------- end --------${quote}
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
MSG_LINKING = ${quote}**** Linking :${quote}
MSG_COMPILING = ${quote}**** Compiling C :${quote}
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
MSG_CLEANING = ${quote}Cleaning project:${quote}
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
# Default target.
#all: begin gccversion sizebefore build sizeafter finished end
#all: begin gencode gccversion build sizeafter finished end
all: elf
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Test if quotes are needed for the echo-command
result = ${shell echo "test"}
ifeq (${result}, test)
quote = '
else
quote =
endif
# Generate intermediate code
gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h
getmodname = $(firstword $(subst /, ,$1))
MOD_GEN := $(foreach MOD,$(MODULES),$(call getmodname,$(MOD)))
# Generate code for module initialization
${OUTDIR}/InitMods.c: Makefile.osx
echo ${MOD_GEN}
@echo ${MSG_MODINIT}
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MOD_GEN}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MOD_GEN}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MOD_GEN}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MOD_GEN}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)
@echo ${MSG_PYMITEINIT}
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
# Eye candy.
begin:
## @echo
@echo $(MSG_BEGIN)
finished:
## @echo $(MSG_ERRORS_NONE)
end:
@echo $(MSG_END)
## @echo
# Display sizes of sections.
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
sizebefore:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
sizeafter:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
@echo $(MSG_SIZE_AFTER)
$(ELFSIZE)
# Display compiler version information.
gccversion :
@$(CC) --version
# @echo $(ALLOBJ)
# Program the device.
ifeq ($(USE_BOOTLOADER), YES)
# Program the device with OP Upload Tool".
program: $(OUTDIR)/$(TARGET).bin
@echo ${quote}Programming with OP Upload Tool${quote}
../../ground/src/experimental/upload-build-desktop/debug/OPUploadTool -d 0 -p $(OUTDIR)/$(TARGET).bin
else
ifeq ($(FLASH_TOOL),OPENOCD)
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
program: $(OUTDIR)/$(TARGET).elf
@echo ${quote}Programming with OPENOCD${quote}
$(OOCD_EXE) $(OOCD_CL)
endif
endif
# Create final output file (.hex) from ELF output file.
%.hex: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O ihex $< $@
# Create final output file (.bin) from ELF output file.
%.bin: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O binary $< $@
# Create extended listing file/disassambly from ELF output file.
# using objdump testing: option -C
%.lss: %.elf
## @echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S -C -r $< > $@
# $(OBJDUMP) -x -S $< > $@
# Create a symbol table from ELF output file.
%.sym: %.elf
## @echo
@echo $(MSG_SYMBOL_TABLE) $@
$(NM) -n $< > $@
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(ALLOBJ)
%.elf: $(ALLOBJ)
@echo $(MSG_LINKING) $@
# use $(CC) for C-only projects or $(CPP) for C++-projects:
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# Assemble: create object files from assembler source files.
define ASSEMBLE_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING) $$< to $$@
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
define ASSEMBLE_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
$(CC) -c $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
define COMPILE_C_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
define COMPILE_C_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
define COMPILE_CPP_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
define COMPILE_CPP_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(SRC:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC) $< to $@
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Compile: create assembler files from C source files. ARM only
$(SRCARM:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC_ARM) $< to $@
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Target: clean project.
clean: begin clean_list finished end
clean_list :
## @echo
@echo $(MSG_CLEANING)
$(REMOVE) $(OUTDIR)/$(TARGET).map
$(REMOVE) $(OUTDIR)/$(TARGET).elf
$(REMOVE) $(OUTDIR)/$(TARGET).hex
$(REMOVE) $(OUTDIR)/$(TARGET).bin
$(REMOVE) $(OUTDIR)/$(TARGET).sym
$(REMOVE) $(OUTDIR)/$(TARGET).lss
$(REMOVE) $(wildcard $(OUTDIR)/*.c)
$(REMOVE) $(wildcard $(OUTDIR)/*.h)
$(REMOVE) $(ALLOBJ)
$(REMOVE) $(LSTFILES)
$(REMOVE) $(DEPFILES)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRCARM:.c=.s)
$(REMOVE) $(CPPSRC:.cpp=.s)
$(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(OUTDIR) 2>NUL)
else
$(shell mkdir $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion \
build elf hex bin lss sym clean clean_list program gencode

210
flight/RevoMini/System/alarms.c Normal file
View File

@ -0,0 +1,210 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @brief OpenPilot System libraries are available to all OP modules.
* @{
* @file alarms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Library for setting and clearing system alarms
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "alarms.h"
// Private constants
// Private types
// Private variables
static xSemaphoreHandle lock;
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
/**
* Initialize the alarms library
*/
int32_t AlarmsInitialize(void)
{
SystemAlarmsInitialize();
lock = xSemaphoreCreateRecursiveMutex();
return 0;
}
/**
* Set an alarm
* @param alarm The system alarm to be modified
* @param severity The alarm severity
* @return 0 if success, -1 if an error
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return -1;
}
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if ( alarms.Alarm[alarm] != severity )
{
alarms.Alarm[alarm] = severity;
SystemAlarmsSet(&alarms);
}
// Release lock
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* Get an alarm
* @param alarm The system alarm to be read
* @return Alarm severity
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return 0;
}
// Read alarm
SystemAlarmsGet(&alarms);
return alarms.Alarm[alarm];
}
/**
* Set an alarm to it's default value
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);
}
/**
* Default all alarms
*/
void AlarmsDefaultAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsDefault(n);
}
}
/**
* Clear an alarm
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
}
/**
* Clear all alarms
*/
void AlarmsClearAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsClear(n);
}
}
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasWarnings()
{
return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
}
/**
* Check if there are any alarms with error or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasErrors()
{
return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
};
/**
* Check if there are any alarms with critical or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasCritical()
{
return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
};
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
uint32_t n;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
if ( alarms.Alarm[n] >= severity)
{
xSemaphoreGiveRecursive(lock);
return 1;
}
}
// If this point is reached then no alarms found
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* @}
* @}
*/

87
flight/RevoMini/System/cm3_fault_handlers.c Normal file
View File

@ -0,0 +1,87 @@
/*
* cm3_fault_handlers.c
*
* Created on: Apr 24, 2011
* Author: msmith
*/
#include <stdint.h>
#include "dcc_stdio.h"
#ifdef STM32F4XX
# include "stm32f4xx.h"
#endif
#ifdef STM32F2XX
# include "stm32f2xx.h"
#endif
#define FAULT_TRAMPOLINE(_vec) \
__attribute__((naked, no_instrument_function)) \
void \
_vec##_Handler(void) \
{ \
__asm( ".syntax unified\n" \
"MOVS R0, #4 \n" \
"MOV R1, LR \n" \
"TST R0, R1 \n" \
"BEQ 1f \n" \
"MRS R0, PSP \n" \
"B " #_vec "_Handler2 \n" \
"1: \n" \
"MRS R0, MSP \n" \
"B " #_vec "_Handler2 \n" \
".syntax divided\n"); \
} \
struct hack
struct cm3_frame {
uint32_t r0;
uint32_t r1;
uint32_t r2;
uint32_t r3;
uint32_t r12;
uint32_t lr;
uint32_t pc;
uint32_t psr;
};
FAULT_TRAMPOLINE(HardFault);
FAULT_TRAMPOLINE(BusFault);
FAULT_TRAMPOLINE(UsageFault);
/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */
#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg))
void
HardFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nHARD FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(HFSR));
dbg_write_char('\n');
for (;;);
}
void
BusFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nBUS FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(CFSR));
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(BFAR));
dbg_write_char('\n');
for (;;);
}
void
UsageFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nUSAGE FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(CFSR));
dbg_write_char('\n');
for (;;);
}

149
flight/RevoMini/System/dcc_stdio.c Normal file
View File

@ -0,0 +1,149 @@
/***************************************************************************
* Copyright (C) 2008 by Dominic Rath *
* Dominic.Rath@gmx.de *
* Copyright (C) 2008 by Spencer Oliver *
* spen@spen-soft.co.uk *
* Copyright (C) 2008 by Frederik Kriewtz *
* frederik@kriewitz.eu *
* *
* This program 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 2 of the License, or *
* (at your option) any later version. *
* *
* This program 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 this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "dcc_stdio.h"
#define TARGET_REQ_TRACEMSG 0x00
#define TARGET_REQ_DEBUGMSG_ASCII 0x01
#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8))
#define TARGET_REQ_DEBUGCHAR 0x02
/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel
* DCRDR[7:0] is used by target for status
* DCRDR[15:8] is used by target for write buffer
* DCRDR[23:16] is used for by host for status
* DCRDR[31:24] is used for by host for write buffer */
#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8))
#define BUSY 1
void dbg_write(unsigned long dcc_data)
{
int len = 4;
while (len--)
{
/* wait for data ready */
while (NVIC_DBG_DATA_R & BUSY);
/* write our data and set write flag - tell host there is data*/
NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY);
dcc_data >>= 8;
}
}
void dbg_trace_point(unsigned long number)
{
dbg_write(TARGET_REQ_TRACEMSG | (number << 8));
}
void dbg_write_u32(const unsigned long *val, long len)
{
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16));
while (len > 0)
{
dbg_write(*val);
val++;
len--;
}
}
void dbg_write_u16(const unsigned short *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = val[0]
| ((len > 1) ? val[1] << 16: 0x0000);
dbg_write(dcc_data);
val += 2;
len -= 2;
}
}
void dbg_write_u8(const unsigned char *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = val[0]
| ((len > 1) ? val[1] << 8 : 0x00)
| ((len > 2) ? val[2] << 16 : 0x00)
| ((len > 3) ? val[3] << 24 : 0x00);
dbg_write(dcc_data);
val += 4;
len -= 4;
}
}
void dbg_write_str(const char *msg)
{
long len;
unsigned long dcc_data;
for (len = 0; msg[len] && (len < 65536); len++);
dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = msg[0]
| ((len > 1) ? msg[1] << 8 : 0x00)
| ((len > 2) ? msg[2] << 16 : 0x00)
| ((len > 3) ? msg[3] << 24 : 0x00);
dbg_write(dcc_data);
msg += 4;
len -= 4;
}
}
void dbg_write_char(char msg)
{
dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16));
}
void dbg_write_hex32(const unsigned long val)
{
static const char hextab[] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
};
for (int shift = 28; shift >= 0; shift -= 4)
dbg_write_char(hextab[(val >> shift) & 0xf]);
}

103
flight/RevoMini/System/inc/FreeRTOSConfig.h Normal file
View File

@ -0,0 +1,103 @@
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
/**
* @addtogroup PIOS PIOS
* @{
* @addtogroup FreeRTOS FreeRTOS
* @{
*/
/* Notes: We use 5 task priorities */
#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ...
#define configTICK_RATE_HZ ((portTickType )1000)
#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5)
#define configMINIMAL_STACK_SIZE ((unsigned short)512)
#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total
#define configMAX_TASK_NAME_LEN (16)
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 1
#define configUSE_TICK_HOOK 0
#define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 0
#define configUSE_MUTEXES 1
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configQUEUE_REGISTRY_SIZE 10
#define configUSE_TIMERS 1
#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
//#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 1
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255
(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */
/* This is the value being used as per the ST library which permits 16
priority values, 0 to 15. This must correspond to the
configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest
NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
/* Enable run time stats collection */
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
/*
* Once we move to CMSIS2 we can at least use:
*
* CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
*
* (still nothing for the DWT registers, surprisingly)
*/
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \
} while(0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
/**
* @}
*/
#endif /* FREERTOS_CONFIG_H */

Some files were not shown because too many files have changed in this diff Show More