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

OP-936: Merges branch 'next' into hyper/OP-936_task-monitor-rework, fixes damage/conflicts,

Browse Source 
and brings the callback scheduler into the fold.

+review OPReview-461
This commit is contained in:
Richard Flay (Hyper) 2013-05-06 19:11:14 +09:30
commit fbc8bc698f
273 changed files with 19837 additions and 12811 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
Makefile
View File

@ -261,9 +261,9 @@ fw_$(1)_%: uavobjects_flight
$(V1) $(MKDIR) -p $(BUILD_DIR)/fw_$(1)/dep
$(V1) cd $(ROOT_DIR)/flight/targets/boards/$(1)/firmware && \
$$(MAKE) -r --no-print-directory \
BUILD_TYPE=fw \
BOARD_NAME=$(1) \
BOARD_SHORT_NAME=$(2) \
BUILD_TYPE=fw \
TOPDIR=$(ROOT_DIR)/flight/targets/boards/$(1)/firmware \
OUTDIR=$(BUILD_DIR)/fw_$(1) \
TARGET=fw_$(1) \
@ -288,9 +288,9 @@ bl_$(1)_%:
$(V1) $(MKDIR) -p $(BUILD_DIR)/bl_$(1)/dep
$(V1) cd $(ROOT_DIR)/flight/targets/boards/$(1)/bootloader && \
$$(MAKE) -r --no-print-directory \
BUILD_TYPE=bl \
BOARD_NAME=$(1) \
BOARD_SHORT_NAME=$(2) \
BUILD_TYPE=bl \
TOPDIR=$(ROOT_DIR)/flight/targets/boards/$(1)/bootloader \
OUTDIR=$(BUILD_DIR)/bl_$(1) \
TARGET=bl_$(1) \
@ -326,9 +326,9 @@ bu_$(1)_%: bl_$(1)_bino
$(V1) $(MKDIR) -p $(BUILD_DIR)/bu_$(1)/dep
$(V1) cd $(ROOT_DIR)/flight/targets/common/bootloader_updater && \
$$(MAKE) -r --no-print-directory \
BUILD_TYPE=bu \
BOARD_NAME=$(1) \
BOARD_SHORT_NAME=$(2) \
BUILD_TYPE=bu \
TOPDIR=$(ROOT_DIR)/flight/targets/common/bootloader_updater \
OUTDIR=$(BUILD_DIR)/bu_$(1) \
TARGET=bu_$(1) \
@ -350,9 +350,9 @@ ef_$(1)_%: bl_$(1)_bin fw_$(1)_opfw
$(V1) $(MKDIR) -p $(BUILD_DIR)/ef_$(1)
$(V1) cd $(ROOT_DIR)/flight/targets/common/entire_flash && \
$$(MAKE) -r --no-print-directory \
BUILD_TYPE=ef \
BOARD_NAME=$(1) \
BOARD_SHORT_NAME=$(2) \
BUILD_TYPE=ef \
DFU_CMD="$(DFUUTIL_DIR)/bin/dfu-util" \
TOPDIR=$(ROOT_DIR)/flight/targets/common/entire_flash \
OUTDIR=$(BUILD_DIR)/ef_$(1) \
@ -656,20 +656,9 @@ ut_$(1)_%: $$(UT_OUT_DIR)
$(V1) cd $(ROOT_DIR)/flight/tests/$(1) && \
$$(MAKE) -r --no-print-directory \
BUILD_TYPE=ut \
BOARD_SHORT_NAME=$(1) \
TCHAIN_PREFIX="" \
REMOVE_CMD="$(RM)" \
\
TARGET=$(1) \
TOPDIR=$(ROOT_DIR)/flight/tests/$(1) \
OUTDIR="$(UT_OUT_DIR)/$(1)" \
\
PIOS=$(PIOS) \
OPUAVOBJ=$(OPUAVOBJ) \
OPUAVTALK=$(OPUAVTALK) \
FLIGHTLIB=$(FLIGHTLIB) \
\
GTEST_DIR=$(GTEST_DIR) \
\
TARGET=$(1) \
$$*
.PHONY: ut_$(1)_clean
@ -847,6 +836,7 @@ docs_all_clean:
.PHONY: build-info
build-info:
@$(ECHO) " BUILD-INFO $(call toprel, $(BUILD_DIR)/$@.txt)"
$(V1) $(MKDIR) -p $(BUILD_DIR)
$(V1) $(VERSION_INFO) \
--uavodir=$(ROOT_DIR)/shared/uavobjectdefinition \
@ -865,7 +855,8 @@ build-info:
help:
@$(ECHO)
@$(ECHO) " This Makefile is known to work on Linux and Mac in a standard shell environment."
@$(ECHO) " It also works on Windows by following the instructions in make/winx86/README.txt."
@$(ECHO) " It also works on Windows by following the instructions given on this wiki page:"
@$(ECHO) " http://wiki.openpilot.org/display/Doc/Windows%3A+Building+and+Packaging"
@$(ECHO)
@$(ECHO) " Here is a summary of the available targets:"
@$(ECHO)
@ -874,12 +865,18 @@ help:
@$(ECHO) " qt_sdk_install - Install the QT development tools"
@$(ECHO) " mingw_install - Install the MinGW toolchain (Windows only)"
@$(ECHO) " python_install - Install the Python interpreter (Windows only)"
@$(ECHO) " nsis_install - Install the NSIS Unicode (Windows only)"
@$(ECHO) " uncrustify_install - Install the Uncrustify source code beautifier"
@$(ECHO) " doxygen_install - Install the Doxygen documentation generator"
@$(ECHO) " gtest_install - Install the GoogleTest framework"
@$(ECHO) " These targets are not updated yet and are probably broken:"
@$(ECHO) " openocd_install - Install the OpenOCD JTAG daemon"
@$(ECHO) " stm32flash_install - Install the stm32flash tool for unbricking F1-based boards"
@$(ECHO) " dfuutil_install - Install the dfu-util tool for unbricking F4-based boards"
@$(ECHO) " android_sdk_install - Install the Android SDK tools"
@$(ECHO) " Install all available tools:"
@$(ECHO) " all_sdk_install - Install all of above (platform-dependent)"
@$(ECHO) " build_sdk_install - Install only essential for build tools (platform-dependent)"
@$(ECHO)
@$(ECHO) " Other tool options are:"
@$(ECHO) " <tool>_version - Display <tool> version"
@ -908,32 +905,32 @@ help:
@$(ECHO)
@$(ECHO) " [Firmware]"
@$(ECHO) " <board> - Build firmware for <board>"
@$(ECHO) " supported boards are ($(ALL_BOARDS))"
@$(ECHO) " Supported boards are ($(ALL_BOARDS))"
@$(ECHO) " fw_<board> - Build firmware for <board>"
@$(ECHO) " supported boards are ($(FW_BOARDS))"
@$(ECHO) " Supported boards are ($(FW_BOARDS))"
@$(ECHO) " fw_<board>_clean - Remove firmware for <board>"
@$(ECHO) " fw_<board>_program - Use OpenOCD + JTAG to write firmware to <board>"
@$(ECHO)
@$(ECHO) " [Bootloader]"
@$(ECHO) " bl_<board> - Build bootloader for <board>"
@$(ECHO) " supported boards are ($(BL_BOARDS))"
@$(ECHO) " Supported boards are ($(BL_BOARDS))"
@$(ECHO) " bl_<board>_clean - Remove bootloader for <board>"
@$(ECHO) " bl_<board>_program - Use OpenOCD + JTAG to write bootloader to <board>"
@$(ECHO)
@$(ECHO) " [Entire Flash]"
@$(ECHO) " ef_<board> - Build entire flash image for <board>"
@$(ECHO) " supported boards are ($(EF_BOARDS))"
@$(ECHO) " Supported boards are ($(EF_BOARDS))"
@$(ECHO) " ef_<board>_clean - Remove entire flash image for <board>"
@$(ECHO) " ef_<board>_program - Use OpenOCD + JTAG to write entire flash image to <board>"
@$(ECHO)
@$(ECHO) " [Bootloader Updater]"
@$(ECHO) " bu_<board> - Build bootloader updater for <board>"
@$(ECHO) " supported boards are ($(BU_BOARDS))"
@$(ECHO) " Supported boards are ($(BU_BOARDS))"
@$(ECHO) " bu_<board>_clean - Remove bootloader updater for <board>"
@$(ECHO)
@$(ECHO) " [Unbrick a board]"
@$(ECHO) " unbrick_<board> - Use the STM32's built in boot ROM to write a bootloader to <board>"
@$(ECHO) " supported boards are ($(BL_BOARDS))"
@$(ECHO) " Supported boards are ($(BL_BOARDS))"
@$(ECHO) " [Unittests]"
@$(ECHO) " ut_<test> - Build unit test <test>"
@$(ECHO) " ut_<test>_xml - Run test and capture XML output into a file"
@ -942,14 +939,14 @@ help:
@$(ECHO) " [Simulation]"
@$(ECHO) " sim_osx - Build OpenPilot simulation firmware for OSX"
@$(ECHO) " sim_osx_clean - Delete all build output for the osx simulation"
@$(ECHO) " sim_win32 - Build OpenPilot simulation firmware for"
@$(ECHO) " Windows using mingw and msys"
@$(ECHO) " sim_win32 - Build OpenPilot simulation firmware for Windows"
@$(ECHO) " using mingw and msys"
@$(ECHO) " sim_win32_clean - Delete all build output for the win32 simulation"
@$(ECHO)
@$(ECHO) " [GCS]"
@$(ECHO) " gcs - Build the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO) " gcs_all_clean - Remove the Ground Control System (GCS) application (all build confgurations)"
@$(ECHO)
@$(ECHO) " [AndroidGCS]"
@ -960,22 +957,35 @@ help:
@$(ECHO)
@$(ECHO) " [UAVObjects]"
@$(ECHO) " uavobjects - Generate source files from the UAVObject definition XML files"
@$(ECHO) " uavobjects_test - parse xml-files - check for valid, duplicate ObjId's, ... "
@$(ECHO) " uavobjects_test - Parse xml-files - check for valid, duplicate ObjId's, ..."
@$(ECHO) " uavobjects_<group> - Generate source files from a subset of the UAVObject definition XML files"
@$(ECHO) " supported groups are ($(UAVOBJ_TARGETS))"
@$(ECHO) " Supported groups are ($(UAVOBJ_TARGETS))"
@$(ECHO)
@$(ECHO) " [Packaging]"
@$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS"
@$(ECHO) " clean_package - Clean, build and package the OpenPilot platform-dependent package"
@$(ECHO) " package - Build and package the OpenPilot platform-dependent package"
@$(ECHO) " package - Build and package the OpenPilot platform-dependent package (no clean)"
@$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS"
@$(ECHO)
@$(ECHO) " [Code Formatting]"
@$(ECHO) " uncrustify_<source> - Reformat <source> code. <source> can be flight or ground"
@$(ECHO) " uncrustify_<source> - Reformat <source> code according to the project's standards"
@$(ECHO) " Supported sources are ($(UNCRUSTIFY_TARGETS))"
@$(ECHO) " uncrustify_all - Reformat all source code"
@$(ECHO)
@$(ECHO) " [Code Documentation]"
@$(ECHO) " docs_<source> - Generate HTML documentation for <source>"
@$(ECHO) " Supported sources are ($(DOCS_TARGETS))"
@$(ECHO) " docs_all - Generate HTML documentation for all"
@$(ECHO) " docs_<source>_clean - Delete generated documentation for <source>"
@$(ECHO) " docs_all_clean - Delete all generated documentation"
@$(ECHO)
@$(ECHO) " Hint: Add V=1 to your command line to see verbose build output."
@$(ECHO)
@$(ECHO) " Notes: All tool distribution files will be downloaded into $(DL_DIR)"
@$(ECHO) " All tools will be installed into $(TOOLS_DIR)"
@$(ECHO) " All build output will be placed in $(BUILD_DIR)"
@$(ECHO) " Notes: All tool distribution files will be downloaded into $(DL_DIR)"
@$(ECHO) " All tools will be installed into $(TOOLS_DIR)"
@$(ECHO) " All build output will be placed in $(BUILD_DIR)"
@$(ECHO)
@$(ECHO) " Tool download and install directories can be changed using environment variables:"
@$(ECHO) " OPENPILOT_DL_DIR full path to downloads directory [downloads if not set]"
@$(ECHO) " OPENPILOT_TOOLS_DIR full path to installed tools directory [tools if not set]"
@$(ECHO) " More info: http://wiki.openpilot.org/display/Doc/OpenPilot+Build+System+Overview"
@$(ECHO)

38
androidgcs/res/layout/mycustommapview.xml Normal file
View File

@ -0,0 +1,38 @@
<?xml version="1.0" encoding="utf-8"?>
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
android:orientation="vertical"
android:layout_width="fill_parent"
android:layout_height="fill_parent"
>
<LinearLayout
android:orientation="vertical"
android:layout_width="fill_parent"
android:layout_height="wrap_content"
>
<TextView
android:id="@+id/longitude"
android:layout_width="fill_parent"
android:layout_height="fill_parent"
android:text="@string/longitude"
/>
<TextView
android:id="@+id/latitude"
android:layout_width="fill_parent"
android:layout_height="fill_parent"
android:text="@string/latitude"
/>
<TextView
android:id="@+id/altitude"
android:layout_width="fill_parent"
android:layout_height="fill_parent"
android:text="@string/altitude"
/>
</LinearLayout>
<org.openpilot.androidgcs.MyCustomMapView
android:id="@+id/mapview"
android:layout_width="fill_parent"
android:layout_height="fill_parent"
android:clickable="true"
android:apiKey="098Goj3psJ2Vg_lGi2v0pRWf4mqxjBvh2FI4frg"
/>
</LinearLayout>

9
androidgcs/res/menu/map_menu.xml Normal file
View File

@ -0,0 +1,9 @@
<?xml version="1.0" encoding="utf-8"?>
<menu xmlns:android="http://schemas.android.com/apk/res/android" >
<item android:id="@+id/poi1" android:title="Set POI"></item>
<item android:id="@+id/poi2" android:title="Set FollowMe POI"></item>
<item android:id="@+id/view1" android:title="View Satellite images"></item>
<item android:id="@+id/view2" android:title="View Map"></item>
</menu>

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

@ -32,6 +32,9 @@
<string name="alarms">Alarms</string>
<string name="txrate">TxRate: </string>
<string name="rxrate">RxRate: </string>
<string name="latitude">Latitude:</string>
<string name="longitude">Longitude:</string>
<string name="altitude">Altitude:</string>
<string name="tester">Tester</string>
<string name="_3dview">3DView</string>
<string name="tuning">Tuning</string>

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

@ -66,12 +66,14 @@ public class Controller extends ObjectManagerActivity {
private boolean leftJoystickHeld, rightJoystickHeld;
Timer sendTimer = new Timer();
TextView manualView;
/** Called when the activity is first created. */
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.controller);
manualView = (TextView) findViewById(R.id.manualControlValues);
}
Observer settingsUpdated = new Observer() {

121
androidgcs/src/org/openpilot/androidgcs/MyCustomMapView.java Normal file
View File

@ -0,0 +1,121 @@
package org.openpilot.androidgcs;
import java.util.Timer;
import java.util.TimerTask;
import com.google.android.maps.GeoPoint;
import com.google.android.maps.MapView;
import android.content.Context;
import android.util.AttributeSet;
import android.view.MotionEvent;
public class MyCustomMapView extends MapView {
// Define the interface we will interact with from our Map
public interface OnLongpressListener {
public void onLongpress(MapView view, GeoPoint longpressLocation);
}
/**
* Time in ms before the OnLongpressListener is triggered.
*/
static final int LONGPRESS_THRESHOLD = 500;
/**
* Keep a record of the center of the map, to know if the map
* has been panned.
*/
private GeoPoint lastMapCenter;
private Timer longpressTimer = new Timer();
private MyCustomMapView.OnLongpressListener longpressListener;
public MyCustomMapView(Context context, String apiKey) {
super(context, apiKey);
}
public MyCustomMapView(Context context, AttributeSet attrs) {
super(context, attrs);
}
public MyCustomMapView(Context context, AttributeSet attrs, int defStyle) {
super(context, attrs, defStyle);
}
public void setOnLongpressListener(MyCustomMapView.OnLongpressListener listener) {
longpressListener = listener;
}
/**
* This method is called every time user touches the map,
* drags a finger on the map, or removes finger from the map.
*/
@Override
public boolean onTouchEvent(MotionEvent event) {
handleLongpress(event);
return super.onTouchEvent(event);
}
/**
* This method takes MotionEvents and decides whether or not
* a longpress has been detected. This is the meat of the
* OnLongpressListener.
*
* The Timer class executes a TimerTask after a given time,
* and we start the timer when a finger touches the screen.
*
* We then listen for map movements or the finger being
* removed from the screen. If any of these events occur
* before the TimerTask is executed, it gets cancelled. Else
* the listener is fired.
*
* @param event
*/
private void handleLongpress(final MotionEvent event) {
if (event.getAction() == MotionEvent.ACTION_DOWN) {
// Finger has touched screen.
longpressTimer = new Timer();
longpressTimer.schedule(new TimerTask() {
@Override
public void run() {
GeoPoint longpressLocation = getProjection().fromPixels((int)event.getX(),
(int)event.getY());
/*
* Fire the listener. We pass the map location
* of the longpress as well, in case it is needed
* by the caller.
*/
longpressListener.onLongpress(MyCustomMapView.this, longpressLocation);
}
}, LONGPRESS_THRESHOLD);
lastMapCenter = getMapCenter();
}
if (event.getAction() == MotionEvent.ACTION_MOVE) {
if (!getMapCenter().equals(lastMapCenter)) {
// User is panning the map, this is no longpress
longpressTimer.cancel();
}
lastMapCenter = getMapCenter();
}
if (event.getAction() == MotionEvent.ACTION_UP) {
// User has removed finger from map.
longpressTimer.cancel();
}
if (event.getPointerCount() > 1) {
// This is a multitouch event, probably zooming.
longpressTimer.cancel();
}
}
}

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

@ -49,13 +49,22 @@ import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Paint;
import android.graphics.Point;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.os.Bundle;
import android.os.Handler;
import android.os.IBinder;
import android.util.Log;
import android.view.ContextMenu;
import android.view.ContextMenu.ContextMenuInfo;
import android.view.Menu;
import android.view.MenuInflater;
import android.view.MenuItem;
import android.view.KeyEvent;
import android.view.View;
import android.widget.TextView;
import android.widget.Toast;
import com.google.android.maps.GeoPoint;
import com.google.android.maps.MapActivity;
@ -72,9 +81,11 @@ public class UAVLocation extends MapActivity
// private static boolean WARN = LOGLEVEL > 1;
private static boolean DEBUG = LOGLEVEL > 0;
private MapView mapView;
//private MapView mapView;
private MyCustomMapView mapView;
private MapController mapController;
private GeoPoint mContextMenuGeoPoint = null;
UAVObjectManager objMngr;
boolean mBound = false;
boolean mConnected = false;
@ -84,23 +95,37 @@ public class UAVLocation extends MapActivity
GeoPoint homeLocation;
GeoPoint uavLocation;
boolean towerEnabled;
boolean gpsEnabled;
LocationManager gpsLocationManager;
LocationManager towerLocationManager;
MyLocationListener gpsLocationListener;
MyLocationListener towerLocationListener;
private TextView myLongitude, myLatitude, myAltitude;
@Override public void onCreate(Bundle icicle) {
super.onCreate(icicle);
setContentView(R.layout.map_layout);
mapView = (MapView)findViewById(R.id.map_view);
super.onCreate(icicle);
setContentView(R.layout.mycustommapview);
mapView = (MyCustomMapView)findViewById(R.id.mapview);
mapController = mapView.getController();
registerForContextMenu(mapView);
mapView.displayZoomControls(true);
Double lat = 37.422006*1E6;
Double lng = -122.084095*1E6;
homeLocation = new GeoPoint(lat.intValue(), lng.intValue());
uavLocation = homeLocation;
mapController.setCenter(homeLocation);
mapController.setZoom(18);
//mapController.setCenter(homeLocation);
mapController.setZoom(16);
List<Overlay> overlays = mapView.getOverlays();
UAVOverlay myOverlay = new UAVOverlay();
overlays.add(myOverlay);
myLongitude = (TextView)findViewById(R.id.longitude);
myLatitude = (TextView)findViewById(R.id.latitude);
myAltitude = (TextView)findViewById(R.id.altitude);
MyLocationOverlay myLocationOverlay = new MyLocationOverlay(this, mapView);
myLocationOverlay.enableMyLocation();
@ -108,15 +133,159 @@ public class UAVLocation extends MapActivity
overlays.add(myLocationOverlay);
mapView.postInvalidate();
gpsLocationListener = new MyLocationListener();
towerLocationListener = new MyLocationListener();
gpsLocationManager = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
towerLocationManager = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
CheckTowerAndGpsStatus();
if(gpsEnabled)
{
gpsLocationManager.requestLocationUpdates( LocationManager.GPS_PROVIDER, 0, 0, gpsLocationListener);
//Get the current location in start-up
Location last = gpsLocationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER);
if(last != null)
{
GeoPoint initGeoPoint = new GeoPoint(
(int)(last.getLatitude()*1000000),
(int)(last.getLongitude()*1000000));
CenterLocation(initGeoPoint,(last.getAltitude()));
}
}
mapView.setOnLongpressListener(new MyCustomMapView.OnLongpressListener() {
public void onLongpress(final MapView view, final GeoPoint longpressLocation) {
mContextMenuGeoPoint = longpressLocation;
runOnUiThread(new Runnable() {
public void run() {
// Insert your longpress action here
//Toast.makeText(mapView.getContext(), "You pressed here: Lat:", Toast.LENGTH_LONG).show();
openContextMenu(view);
}
});
}
});
}
private void CenterLocation(GeoPoint centerGeoPoint, double Altitude)
{
mapController.animateTo(centerGeoPoint);
myLongitude.setText("Longitude: "+
String.valueOf((float)centerGeoPoint.getLongitudeE6()/1000000)
);
myLatitude.setText("Latitude: "+
String.valueOf((float)centerGeoPoint.getLatitudeE6()/1000000)
);
myAltitude.setText("Altitude: "+
String.valueOf(Altitude)
);
};
private void CheckTowerAndGpsStatus() {
towerEnabled = towerLocationManager
.isProviderEnabled(LocationManager.NETWORK_PROVIDER);
gpsEnabled = gpsLocationManager
.isProviderEnabled(LocationManager.GPS_PROVIDER);
}
//@Override
@Override
@Override
protected boolean isRouteDisplayed() {
// IMPORTANT: This method must return true if your Activity // is displaying driving directions. Otherwise return false.
return false;
}
@Override
public void onCreateContextMenu(ContextMenu menu, View v,
ContextMenuInfo menuInfo) {
super.onCreateContextMenu(menu, v, menuInfo);
MenuInflater inflater = getMenuInflater();
inflater.inflate(R.menu.map_menu, menu);
}
@Override
public boolean onContextItemSelected(MenuItem item) {
int lat = mContextMenuGeoPoint.getLatitudeE6();
int lon = mContextMenuGeoPoint.getLongitudeE6();
switch (item.getItemId()) {
case R.id.poi1:
return true;
case R.id.poi2:
Toast.makeText(mapView.getContext(), "You pressed here: Lat:" + lat/1000000.0 + " Lon:" + lon/1000000.0, Toast.LENGTH_LONG).show();
return true;
case R.id.view1:
mapView.setSatellite(true);
mapView.invalidate();
return true;
case R.id.view2:
mapView.setSatellite(false);
mapView.invalidate();
return true;
default:
return super.onContextItemSelected(item);
}
}
/* Class My Location Listener */
public class MyLocationListener implements LocationListener
{
@Override
public void onLocationChanged(Location loc)
{
GeoPoint myGeoPoint = new GeoPoint(
(int)(loc.getLatitude()*1000000),
(int)(loc.getLongitude()*1000000));
CenterLocation(myGeoPoint,loc.getAltitude());
}
@Override
public void onProviderDisabled(String provider)
{
Toast.makeText( getApplicationContext(),
"Gps Disabled",
Toast.LENGTH_SHORT ).show();
}
@Override
public void onProviderEnabled(String provider)
{
Toast.makeText( getApplicationContext(),
"Gps Enabled",
Toast.LENGTH_SHORT).show();
}
@Override
public void onStatusChanged(String provider, int status, Bundle extras)
{
}
}/* End of Class MyLocationListener */
public boolean onKeyDown(int keyCode, KeyEvent event) {
switch (keyCode) {
case KeyEvent.KEYCODE_DPAD_UP:
mapController.zoomIn();
break;
case KeyEvent.KEYCODE_DPAD_DOWN:
mapController.zoomOut();
break;
case KeyEvent.KEYCODE_DPAD_LEFT:
mapController.setZoom(17);
mapView.setSatellite(true);
mapView.invalidate();
break;
case KeyEvent.KEYCODE_DPAD_RIGHT:
mapController.setZoom(17);
mapView.setSatellite(false);
mapView.invalidate();
break;
}
return super.onKeyDown(keyCode, event);
}
public class UAVOverlay extends Overlay {
Bitmap homeSymbol = BitmapFactory.decodeResource(getResources(), R.drawable.ic_home);

3
flight/.gitattributes vendored
View File

@ -15,3 +15,6 @@
Makefile text eol=lf
Makefile.* text eol=lf
*.mk text eol=lf
*.h.template text eol=lf
*.c.template text eol=lf

8
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -17,8 +17,8 @@
65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = "<group>"; };
65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = "<group>"; };
65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = "<group>"; };
65904F2914632C1700FD9482 /* firmwareinfotemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmwareinfotemplate.c; sourceTree = "<group>"; };
65904F2A14632C1700FD9482 /* gcsversioninfotemplate.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcsversioninfotemplate.h; sourceTree = "<group>"; };
65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = "<group>"; };
65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = "<group>"; };
65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = "<group>"; };
65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = "<group>"; };
65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = "<group>"; };
@ -79,8 +79,8 @@
65904F1A14632C1700FD9482 /* templates */ = {
isa = PBXGroup;
children = (
65904F2914632C1700FD9482 /* firmwareinfotemplate.c */,
65904F2A14632C1700FD9482 /* gcsversioninfotemplate.h */,
65904F2914632C1700FD9482 /* firmware_info.c.template */,
65904F2A14632C1700FD9482 /* gcs_version_info.h.template */,
);
path = templates;
sourceTree = "<group>";

4
flight/Project/gdb/osd Normal file
View File

@ -0,0 +1,4 @@
define connect
target remote localhost:3333
file ./build/fw_osd/fw_osd.elf
end

2
flight/libraries/WorldMagModel.c
View File

@ -834,7 +834,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
* Note: OP code change to avoid floating point equality test.
* Was: if (fabs(x) == 1.0)
*/
if (fabs(x) - 1.0f < 1e-9f)
if (fabsf(x) - 1.0f < 1e-9f)
{
FREE(PreSqr);
FREE(f2);

4
flight/libraries/inc/insgps.h
View File

@ -59,11 +59,13 @@ void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSGetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetPosVelVar(float PosVar[3], float VelVar[3]);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var);

96
flight/libraries/inc/packet_handler.h
View File

@ -1,16 +1,16 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @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
@ -41,66 +41,66 @@
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_CON_REQUEST, // request a connection to another modem
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK, // Acknowlege the receipt of a packet
PACKET_TYPE_ACK_RTS, // Acknowlege the receipt of a packet and indicate that the receiving side has data to send (ready to send)
PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet
PACKET_TYPE_NONE = 0,
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_CON_REQUEST, // request a connection to another modem
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK, // Acknowlege the receipt of a packet
PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet
} PHPacketType;
typedef struct {
uint32_t destination_id;
uint16_t seq_num;
uint8_t type;
uint8_t data_size;
uint32_t destination_id;
portTickType prev_tx_time;
uint16_t seq_num;
uint8_t type;
uint8_t data_size;
} PHPacketHeader;
#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY)
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle;
#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
portTickType packet_recv_time;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint8_t ecc[RS_ECC_NPARITY];
} PHAckNackPacket, *PHAckNackPacketHandle;
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint32_t source_id;
uint8_t link_quality;
int8_t received_rssi;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint32_t source_id;
uint8_t link_quality;
int8_t received_rssi;
uint8_t ecc[RS_ECC_NPARITY];
} PHStatusPacket, *PHStatusPacketHandle;
#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint32_t source_id;
uint32_t min_frequency;
uint32_t max_frequency;
uint32_t channel_spacing;
OPLinkSettingsMainPortOptions main_port;
OPLinkSettingsFlexiPortOptions flexi_port;
OPLinkSettingsVCPPortOptions vcp_port;
OPLinkSettingsComSpeedOptions com_speed;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint32_t source_id;
uint32_t min_frequency;
uint32_t max_frequency;
uint32_t channel_spacing;
portTickType status_rx_time;
OPLinkSettingsMainPortOptions main_port;
OPLinkSettingsFlexiPortOptions flexi_port;
OPLinkSettingsVCPPortOptions vcp_port;
OPLinkSettingsComSpeedOptions com_speed;
uint8_t ecc[RS_ECC_NPARITY];
} PHConnectionPacket, *PHConnectionPacketHandle;
#endif // __PACKET_HANDLER_H__

35
flight/libraries/insgps13state.c
View File

@ -141,7 +141,19 @@ void INSResetP(float PDiag[NUMX])
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
void INSGetP(float PDiag[NUMX])
{
uint8_t i;
// retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){
if (PDiag != 0){
PDiag[i] = P[i][i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
@ -179,14 +191,14 @@ void INSPosVelReset(float pos[3], float vel[3])
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
void INSSetPosVelVar(float PosVar[3], float VelVar[3])
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
R[5] = VelVar;
R[0] = PosVar[0];
R[1] = PosVar[1];
R[2] = PosVar[2];
R[3] = VelVar[0];
R[4] = VelVar[1];
R[5] = VelVar[2];
}
void INSSetGyroBias(float gyro_bias[3])
@ -210,6 +222,13 @@ void INSSetGyroVar(float gyro_var[3])
Q[2] = gyro_var[2];
}
void INSSetGyroBiasVar(float gyro_bias_var[3])
{
Q[6] = gyro_bias_var[0];
Q[7] = gyro_bias_var[1];
Q[8] = gyro_bias_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];

2
flight/libraries/op_dfu.c
View File

@ -78,7 +78,7 @@ extern uint8_t JumpToApp;
void sendData(uint8_t * buf, uint16_t size);
uint32_t CalcFirmCRC(void);
void DataDownload(DownloadAction action) {
void DataDownload(__attribute__((unused)) DownloadAction action) {
if ((DeviceState == downloading)) {
uint8_t packetSize;

267
flight/libraries/paths.c
View File

@ -24,7 +24,9 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#include <pios.h>
#include <pios_math.h>
#include "paths.h"
#include "uavobjectmanager.h" // <--.
@ -32,8 +34,8 @@
// no direct UAVObject usage allowed in this file
// private functions
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
/**
@ -46,26 +48,26 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
*/
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
{
switch(mode) {
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
break;
}
switch (mode) {
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
break;
}
}
/**
@ -75,38 +77,38 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path, dist_diff;
float path_north, path_east, diff_north, diff_east;
float dist_path, dist_diff;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
dist_path = sqrtf( path_north * path_north + path_east * path_east );
dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
dist_path = sqrtf(path_north * path_north + path_east * path_east);
if (dist_diff < 1e-6f ) {
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->error = dist_diff;
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->error = dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
}
@ -117,50 +119,50 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path;
float dot;
float normal[2];
float path_north, path_east, diff_north, diff_east;
float dist_path;
float dot;
float normal[2];
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Current progress location relative to start
diff_north = cur_point[0] - start_point[0];
diff_east = cur_point[1] - start_point[1];
// Current progress location relative to start
diff_north = cur_point[0] - start_point[0];
diff_east = cur_point[1] - start_point[1];
dot = path_north * diff_north + path_east * diff_east;
dist_path = sqrtf( path_north * path_north + path_east * path_east );
dot = path_north * diff_north + path_east * diff_east;
dist_path = sqrtf(path_north * path_north + path_east * path_east);
if (dist_path < 1e-6f){
// if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint( start_point, end_point, cur_point, status );
status->fractional_progress = 1;
return;
}
// if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint(start_point, end_point, cur_point, status);
status->fractional_progress = 1;
return;
}
// Compute the normal to the path
normal[0] = -path_east / dist_path;
normal[1] = path_north / dist_path;
// Compute the normal to the path
normal[0] = -path_east / dist_path;
normal[1] = path_north / dist_path;
status->fractional_progress = dot / (dist_path * dist_path);
status->error = normal[0] * diff_north + normal[1] * diff_east;
status->fractional_progress = dot / (dist_path * dist_path);
status->error = normal[0] * diff_north + normal[1] * diff_east;
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
// Now just want magnitude of error
status->error = fabs(status->error);
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
// Compute direction to travel
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
// Now just want magnitude of error
status->error = fabs(status->error);
// Compute direction to travel
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
}
@ -173,54 +175,79 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
*/
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
{
float radius_north, radius_east, diff_north, diff_east;
float radius,cradius;
float normal[2];
float radius_north, radius_east, diff_north, diff_east;
float radius, cradius;
float normal[2];
float progress;
float a_diff, a_radius;
// Radius
radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1];
// Radius
radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1];
// Current location relative to center
diff_north = cur_point[0] - end_point[0];
diff_east = cur_point[1] - end_point[1];
// Current location relative to center
diff_north = cur_point[0] - end_point[0];
diff_east = cur_point[1] - end_point[1];
radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
cradius = sqrtf( diff_north * diff_north + diff_east * diff_east );
radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
if (cradius < 1e-6f) {
// cradius is zero, just fly somewhere and make sure correction is still a normal
status->fractional_progress = 1;
status->error = radius;
status->correction_direction[0] = 0;
status->correction_direction[1] = 1;
status->path_direction[0] = 1;
status->path_direction[1] = 0;
return;
}
// cradius is zero, just fly somewhere and make sure correction is still a normal
status->fractional_progress = 1;
status->error = radius;
status->correction_direction[0] = 0;
status->correction_direction[1] = 1;
status->path_direction[0] = 1;
status->path_direction[1] = 0;
return;
}
if (clockwise) {
// Compute the normal to the radius clockwise
normal[0] = -diff_east / cradius;
normal[1] = diff_north / cradius;
} else {
// Compute the normal to the radius counter clockwise
normal[0] = diff_east / cradius;
normal[1] = -diff_north / cradius;
}
status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
if (clockwise) {
// Compute the normal to the radius clockwise
normal[0] = -diff_east / cradius;
normal[1] = diff_north / cradius;
} else {
// Compute the normal to the radius counter clockwise
normal[0] = diff_east / cradius;
normal[1] = -diff_north / cradius;
}
// error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
// normalize progress to 0..1
a_diff = atan2f(diff_north, diff_east);
a_radius = atan2f(radius_north, radius_east);
// Compute direction to correct error
status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
if (a_diff < 0) {
a_diff += 2.0f * M_PI_F;
}
if (a_radius < 0) {
a_radius += 2.0f * M_PI_F;
}
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
status->error = fabs(status->error);
if (progress < 0) {
progress += 1.0f;
} else if (progress >= 1.0f) {
progress -= 1.0f;
}
if (clockwise) {
progress = 1 - progress;
}
status->fractional_progress = progress;
// error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->error = fabs(status->error);
}

30
flight/libraries/printf2.c
View File

@ -74,7 +74,7 @@ char **environ = __env;
/*==============================================================================
* Close a file.
*/
int _close(int file)
int _close(__attribute__((unused)) int file)
{
return -1;
}
@ -82,7 +82,7 @@ int _close(int file)
/*==============================================================================
* Transfer control to a new process.
*/
int _execve(char *name, char **argv, char **env)
int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env)
{
errno = ENOMEM;
return -1;
@ -91,7 +91,7 @@ int _execve(char *name, char **argv, char **env)
/*==============================================================================
* Exit a program without cleaning up files.
*/
void _exit( int code )
void _exit( __attribute__((unused)) int code )
{
/* Should we force a system reset? */
while( 1 )
@ -112,7 +112,7 @@ int _fork(void)
/*==============================================================================
* Status of an open file.
*/
int _fstat(int file, struct stat *st)
int _fstat(__attribute__((unused)) int file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
@ -129,7 +129,7 @@ int _getpid(void)
/*==============================================================================
* Query whether output stream is a terminal.
*/
int _isatty(int file)
int _isatty(__attribute__((unused)) int file)
{
return 1;
}
@ -137,7 +137,7 @@ int _isatty(int file)
/*==============================================================================
* Send a signal.
*/
int _kill(int pid, int sig)
int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
{
errno = EINVAL;
return -1;
@ -146,7 +146,7 @@ int _kill(int pid, int sig)
/*==============================================================================
* Establish a new name for an existing file.
*/
int _link(char *old, char *new)
int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
{
errno = EMLINK;
return -1;
@ -155,7 +155,7 @@ int _link(char *old, char *new)
/*==============================================================================
* Set position in a file.
*/
int _lseek(int file, int ptr, int dir)
int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
{
return 0;
}
@ -163,7 +163,7 @@ int _lseek(int file, int ptr, int dir)
/*==============================================================================
* Open a file.
*/
int _open(const char *name, int flags, int mode)
int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
{
return -1;
}
@ -171,7 +171,7 @@ int _open(const char *name, int flags, int mode)
/*==============================================================================
* Read from a file.
*/
int _read(int file, char *ptr, int len)
int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
{
return 0;
}
@ -182,7 +182,7 @@ int _read(int file, char *ptr, int len)
* example to a serial port for debugging, you should make your minimal write
* capable of doing this.
*/
int _write_r( void * reent, int file, char * ptr, int len )
int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len )
{
return 0;
}
@ -220,7 +220,7 @@ caddr_t _sbrk(int incr)
/*==============================================================================
* Status of a file (by name).
*/
int _stat(char *file, struct stat *st)
int _stat(__attribute__((unused)) char *file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
@ -229,7 +229,7 @@ int _stat(char *file, struct stat *st)
/*==============================================================================
* Timing information for current process.
*/
int _times(struct tms *buf)
int _times(__attribute__((unused)) struct tms *buf)
{
return -1;
}
@ -237,7 +237,7 @@ int _times(struct tms *buf)
/*==============================================================================
* Remove a file's directory entry.
*/
int _unlink(char *name)
int _unlink(__attribute__((unused)) char *name)
{
errno = ENOENT;
return -1;
@ -246,7 +246,7 @@ int _unlink(char *name)
/*==============================================================================
* Wait for a child process.
*/
int _wait(int *status)
int _wait(__attribute__((unused)) int *status)
{
errno = ECHILD;
return -1;

52
flight/libraries/sanitycheck.c
View File

@ -63,7 +63,7 @@ int32_t configuration_check()
bool multirotor = true;
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch(airframe_type) {
switch (airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
@ -73,6 +73,7 @@ int32_t configuration_check()
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
multirotor = true;
break;
default:
@ -86,8 +87,8 @@ int32_t configuration_check()
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
for(uint32_t i = 0; i < num_modes; i++) {
switch(modes[i]) {
for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
@ -122,9 +123,42 @@ int32_t configuration_check()
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol){
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports Position Hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports AutoLand Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports POI Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
@ -141,7 +175,7 @@ int32_t configuration_check()
}
// TODO: Check on a multirotor no axis supports "None"
if(severity != SYSTEMALARMS_ALARM_OK)
if (severity != SYSTEMALARMS_ALARM_OK)
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
else
AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
@ -159,13 +193,13 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
return SYSTEMALARMS_ALARM_ERROR;
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch(index) {
switch (index) {
case 1:
ManualControlSettingsStabilization1SettingsGet(modes);
break;
@ -181,7 +215,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for(uint32_t i = 0; i < NELEMENTS(modes); i++) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE)
return SYSTEMALARMS_ALARM_ERROR;
}

21
flight/modules/Actuator/actuator.c
View File

@ -152,12 +152,13 @@ MODULE_INITCALL(ActuatorInitialize, ActuatorStart)
*
* @return -1 if error, 0 if success
*/
static void actuatorTask(void* parameters)
static void actuatorTask(__attribute__((unused)) void* parameters)
{
UAVObjEvent ev;
portTickType lastSysTime;
portTickType thisSysTime;
float dT = 0.0f;
float dTSeconds;
uint32_t dTMilliseconds;
ActuatorCommandData command;
ActuatorDesiredData desired;
@ -208,13 +209,9 @@ static void actuatorTask(void* parameters)
// Check how long since last update
thisSysTime = xTaskGetTickCount();
// reuse dt in case of wraparound
// todo:
// if dT actually matters...
// fix it to know max value and subtract for currently correct dT on wrap
if(thisSysTime > lastSysTime)
dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f);
dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS;
lastSysTime = thisSysTime;
dTSeconds = dTMilliseconds * 0.001f;
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
@ -298,7 +295,7 @@ static void actuatorTask(void* parameters)
}
if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
else
status[ct] = -1;
@ -372,7 +369,7 @@ static void actuatorTask(void* parameters)
actuatorSettings.ChannelNeutral[i]);
// Store update time
command.UpdateTime = dT * 1000.0f;
command.UpdateTime = dTMilliseconds;
if (command.UpdateTime > command.MaxUpdateTime)
command.MaxUpdateTime = command.UpdateTime;
@ -723,12 +720,12 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuato
}
}
static void ActuatorSettingsUpdatedCb(UAVObjEvent * ev)
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
actuator_settings_updated = true;
}
static void MixerSettingsUpdatedCb(UAVObjEvent * ev)
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
mixer_settings_updated = true;
}

4
flight/modules/Airspeed/revolution/airspeed.c
View File

@ -131,7 +131,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart)
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
static void airspeedTask(__attribute__((unused)) void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
@ -179,7 +179,7 @@ static void airspeedTask(void *parameters)
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedSettingsGet(&airspeedSettings);

2
flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c
View File

@ -61,7 +61,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
//Check to see if airspeed sensor is returning airspeedSensor
airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (airspeedSensor->SensorValue==-1) {
if (airspeedSensor->SensorValue==(uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
return;

70
flight/modules/Altitude/altitude.c
View File

@ -40,7 +40,9 @@
#include "hwsettings.h"
#include "altitude.h"
#if defined(PIOS_INCLUDE_BMP085)
#include "baroaltitude.h" // object that will be updated by the module
#endif
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
@ -57,12 +59,15 @@
static xTaskHandle taskHandle;
// down sampling variables
#if defined(PIOS_INCLUDE_BMP085)
#define alt_ds_size 4
static int32_t alt_ds_temp = 0;
static int32_t alt_ds_pres = 0;
static int alt_ds_count = 0;
#endif
static bool altitudeEnabled;
static uint8_t hwsettings_rcvrport;;
// Private functions
static void altitudeTask(void *parameters);
@ -75,9 +80,11 @@ int32_t AltitudeStart()
{
if (altitudeEnabled) {
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeInitialize();
#endif
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialze();
SonarAltitudeInitialize();
#endif
// Start main task
@ -107,59 +114,65 @@ int32_t AltitudeInitialize()
}
#endif
#if defined(PIOS_INCLUDE_BMP085)
// init down-sampling data
alt_ds_temp = 0;
alt_ds_pres = 0;
alt_ds_count = 0;
#endif
HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
* Module thread, should not return.
*/
static void altitudeTask(void *parameters)
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroAltitudeData data;
portTickType lastSysTime;
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata;
int32_t value=0,timeout=5;
float coeff=0.25,height_out=0,height_in=0;
PIOS_HCSR04_Init();
PIOS_HCSR04_Trigger();
int32_t value = 0, timeout = 5;
float coeff = 0.25, height_out = 0, height_in = 0;
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
PIOS_HCSR04_Trigger();
}
#endif
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeData data;
PIOS_BMP085_Init();
#endif
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
#if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude (all pressures in kPa)
if(PIOS_HCSR04_Completed())
{
value = PIOS_HCSR04_Get();
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
{
height_in = value*0.00034;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
// Compute the current altitude
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
if(PIOS_HCSR04_Completed()) {
value = PIOS_HCSR04_Get();
//from 3.4cm to 5.1m
if((value > 100) && (value < 15000)) {
height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
}
// Update the AltitudeActual UAVObject
SonarAltitudeSet(&sonardata);
timeout = 5;
PIOS_HCSR04_Trigger();
}
if(!(timeout--)) {
//retrigger
timeout = 5;
PIOS_HCSR04_Trigger();
}
// Update the AltitudeActual UAVObject
SonarAltitudeSet(&sonardata);
timeout=5;
PIOS_HCSR04_Trigger();
}
if(timeout--)
{
//retrigger
timeout=5;
PIOS_HCSR04_Trigger();
}
#endif
#if defined(PIOS_INCLUDE_BMP085)
// Update the temperature data
PIOS_BMP085_StartADC(TemperatureConv);
#ifdef PIOS_BMP085_HAS_GPIOS
@ -198,6 +211,7 @@ static void altitudeTask(void *parameters)
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
}
#endif
// Delay until it is time to read the next sample
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);

57
flight/modules/Altitude/revolution/altitude.c
View File

@ -63,10 +63,6 @@ static void altitudeTask(void *parameters);
*/
int32_t AltitudeStart()
{
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialze();
#endif
// Start main task
xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
@ -81,22 +77,23 @@ int32_t AltitudeStart()
int32_t AltitudeInitialize()
{
BaroAltitudeInitialize();
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
* Module thread, should not return.
*/
static void altitudeTask(void *parameters)
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroAltitudeData data;
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata;
int32_t value=0,timeout=5;
float coeff=0.25,height_out=0,height_in=0;
PIOS_HCSR04_Init();
int32_t value = 0, timeout = 10, sample_rate = 0;
float coeff = 0.25, height_out = 0, height_in = 0;
PIOS_HCSR04_Trigger();
#endif
@ -113,27 +110,29 @@ static void altitudeTask(void *parameters)
while (1)
{
#if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude (all pressures in kPa)
if(PIOS_HCSR04_Completed())
{
value = PIOS_HCSR04_Get();
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
{
height_in = value*0.00034;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
// Compute the current altitude
// depends on baro samplerate
if(!(sample_rate--)) {
if(PIOS_HCSR04_Completed()) {
value = PIOS_HCSR04_Get();
//from 3.4cm to 5.1m
if((value > 100) && (value < 15000)) {
height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
}
// Update the SonarAltitude UAVObject
SonarAltitudeSet(&sonardata);
timeout = 10;
PIOS_HCSR04_Trigger();
}
// Update the AltitudeActual UAVObject
SonarAltitudeSet(&sonardata);
timeout=5;
PIOS_HCSR04_Trigger();
}
if(timeout--)
{
//retrigger
timeout=5;
PIOS_HCSR04_Trigger();
if(!(timeout--)) {
//retrigger
timeout = 10;
PIOS_HCSR04_Trigger();
}
sample_rate = 25;
}
#endif
float temp, press;

4
flight/modules/AltitudeHold/altitudehold.c
View File

@ -119,7 +119,7 @@ float starting_altitude;
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
static void altitudeHoldTask(__attribute__((unused)) void *parameters)
{
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
@ -387,7 +387,7 @@ static void altitudeHoldTask(void *parameters)
}
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
}

34
flight/modules/Attitude/attitude.c
View File

@ -95,6 +95,7 @@ static bool accel_filter_enabled = false;
static float accels_filtered[3];
static float grot_filtered[3];
static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
static float gyroGain = 0.42f;
static int16_t accelbias[3];
static float q[4] = {1,0,0,0};
@ -176,7 +177,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
int32_t accel_test;
int32_t gyro_test;
static void AttitudeTask(void *parameters)
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@ -219,17 +220,19 @@ static void AttitudeTask(void *parameters)
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
// Use accels to initialise attitude and calculate gyro bias
accelKp = 1.0f;
accelKi = 0.9f;
accelKi = 0.0f;
yawBiasRate = 0.01f;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1.0f;
accelKi = 0.9f;
accelKi = 0.0f;
yawBiasRate = 0.01f;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
} else if (init == 0) {
@ -237,6 +240,7 @@ static void AttitudeTask(void *parameters)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f)
accel_filter_enabled = true;
init = 1;
@ -359,7 +363,11 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
gyros->y += gyro_correct_int[1];
gyros->z += gyro_correct_int[2];
}
// Force the roll & pitch gyro rates to average to zero during initialisation
gyro_correct_int[0] += - gyros->x * rollPitchBiasRate;
gyro_correct_int[1] += - gyros->y * rollPitchBiasRate;
// 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] += - gyros->z * yawBiasRate;
@ -431,6 +439,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyrosData->z += gyro_correct_int[2];
}
// Force the roll & pitch gyro rates to average to zero during initialisation
gyro_correct_int[0] += - gyrosData->x * rollPitchBiasRate;
gyro_correct_int[1] += - gyrosData->y * rollPitchBiasRate;
// 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;
@ -460,7 +472,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001f : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
@ -547,10 +559,10 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
q[2] = 0;
q[3] = 0;
} else {
q[0] = q[0]/qmag;
q[1] = q[1]/qmag;
q[2] = q[2]/qmag;
q[3] = q[3]/qmag;
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
}
AttitudeActualData attitudeActual;
@ -564,7 +576,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
AttitudeActualSet(&attitudeActual);
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);

564
flight/modules/Attitude/revolution/attitude.c
View File

@ -65,6 +65,8 @@
#include "homelocation.h"
#include "magnetometer.h"
#include "positionactual.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocityactual.h"
@ -103,10 +105,15 @@ static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings;
static bool gyroBiasSettingsUpdated = false;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
// Private functions
static void AttitudeTask(void *parameters);
@ -116,6 +123,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
// check for invalid values
static inline bool invalid(float data) {
if ( isnan(data) || isinf(data) ){
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data) {
if ( invalid(data) ) {
return true;
}
if ( data < 1e-15f ) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
@ -139,6 +165,9 @@ int32_t AttitudeInitialize(void)
VelocityActualInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
@ -146,24 +175,26 @@ int32_t AttitudeInitialize(void)
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0;
gyrosBias.y = 0;
gyrosBias.z = 0;
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -204,10 +235,9 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
bool first_run = true;
uint32_t last_algorithm;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
@ -216,21 +246,19 @@ static void AttitudeTask(void *parameters)
// Wait for all the sensors be to read
vTaskDelay(100);
// Invalidate previous algorithm to trigger a first run
last_algorithm = 0xfffffff;
// Main task loop
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) {
last_algorithm = revoSettings.FusionAlgorithm;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) {
switch (running_algorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
@ -245,8 +273,9 @@ static void AttitudeTask(void *parameters)
break;
}
if(ret_val == 0)
first_run = false;
if(ret_val != 0) {
initialization_required = true;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
@ -283,8 +312,6 @@ static int32_t updateAttitudeComplementary(bool first_run)
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
@ -294,16 +321,36 @@ static int32_t updateAttitudeComplementary(bool first_run)
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
@ -319,12 +366,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
magKp = 1;
magKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
magKp = 1;
magKp = 1.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
@ -351,8 +398,8 @@ static int32_t updateAttitudeComplementary(bool first_run)
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
@ -388,13 +435,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
if (bmag < 1.0f || mag_len < 1.0f)
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
@ -405,6 +452,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
@ -413,10 +467,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
@ -424,7 +478,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
if(q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
@ -440,11 +494,11 @@ static int32_t updateAttitudeComplementary(bool first_run)
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
}
quat_copy(q, &attitudeActual.q1);
@ -504,7 +558,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0;
}
@ -538,7 +597,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
static bool gps_updated;
static bool gps_vel_updated;
static float baroOffset = 0;
static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0;
static bool inited;
@ -588,8 +649,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
// Check if we are running simulation
if (!GPSPositionReadOnly()) {
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_updated |= pdTRUE && outdoor_mode;
}
if (!GPSVelocityReadOnly()) {
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_vel_updated |= pdTRUE && outdoor_mode;
}
// Get most recent data
GyrosGet(&gyrosData);
@ -601,119 +673,206 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
// Discard mag if it has NAN (normally from bad calibration)
mag_updated &= (!isnan(magData.x) && !isinf(magData.x) && !isnan(magData.y) && !isinf(magData.y) && !isnan(magData.z) && !isinf(magData.z));
value_error = false;
// safety checks
if ( invalid(gyrosData.x) ||
invalid(gyrosData.y) ||
invalid(gyrosData.z) ||
invalid(accelsData.x) ||
invalid(accelsData.y) ||
invalid(accelsData.z) ) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if ( invalid(gyrosBias.x) ||
invalid(gyrosBias.y) ||
invalid(gyrosBias.z) ) {
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
}
if ( invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z) ) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] > 0.0f || homeLocation.Be[1] > 0.0f || homeLocation.Be[2] > 0.0f);
if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) {
mag_updated = false;
value_error = true;
}
if ( invalid(baroData.Altitude) ) {
baro_updated = false;
value_error = true;
}
if ( invalid(airspeedData.CalibratedAirspeed) ) {
airspeed_updated = false;
value_error = true;
}
if ( invalid(gpsData.Altitude) ) {
gps_updated = false;
value_error = true;
}
if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) {
gps_updated = false;
value_error = true;
}
if ( invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down) ) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
if ( ( gpsData.Satellites < 7 ) ||
( gpsData.PDOP > 4.0f ) ||
( gpsData.Latitude==0 && gpsData.Longitude==0 ) ||
( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited)
if ( !inited ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else if (outdoor_mode && gpsData.Satellites < 7)
} else if ( value_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f) {
dT = 0.01f;
} else if(dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read
if (init_stage == 0 && !outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
if (init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } );
INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } );
INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } );
INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } );
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias
float gyro_bias[3] = {0.0f, 0.0f, 0.0f};
INSSetGyroBias(gyro_bias);
float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
if (outdoor_mode) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
AttitudeActualGet (&attitudeActual);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float NED[3];
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Initialize barometric offset to cirrent GPS NED coordinate
baroOffset = -NED[2] - baroData.Altitude;
INSResetP(ekfConfiguration.P);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(NED, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage > 0) {
} else {
// Run prediction a bit before any corrections
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f,
(gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f,
(gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f};
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
@ -728,38 +887,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if(init_stage > 10)
inited = true;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
if (!inited)
return 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f)
dT = 0.01f;
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * M_PI_F / 180.0f;
gyros[1] += gyrosBias.y * M_PI_F / 180.0f;
gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
// Advance the state estimate
@ -788,14 +928,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (gps_updated && outdoor_mode)
{
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] });
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0;
vel[2] = 0.0f;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
@ -806,9 +951,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) {
INSSetPosVelVar(1e6f, 1e5f);
vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0;
INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] });
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS |VERT_SENSORS;
@ -833,11 +983,16 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if ( !gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar(1e6f, 1e2f);
INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] });
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q,R);
float vtas[3] = {airspeed.TrueAirspeed,0,0};
float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f};
rot_mult(R,vtas,vel);
}
}
@ -864,15 +1019,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
GyrosBiasSet(&gyrosBias);
}
gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]);
gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]);
gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]);
GyrosBiasSet(&gyrosBias);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(vardata.P);
EKFStateVarianceSet(&vardata);
return 0;
}
@ -902,24 +1057,58 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
static void settingsUpdatedCb(UAVObjEvent * ev)
{
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
/* When the revo calibration is updated, update the GyroBias object */
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
GyrosBiasSet(&gyrosBias);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL ||
ev->obj == EKFConfigurationHandle() ||
ev->obj == RevoSettingsHandle() ||
( variance_error==true && ev->obj == FlightStatusHandle() )
) {
gyroBiasSettingsUpdated = true;
bool error = false;
// In case INS currently running
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
EKFConfigurationGet(&ekfConfiguration);
int t;
for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.P[t])) {
error = true;
}
}
for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q[t])) {
error = true;
}
}
for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) {
error = true;
}
if (error) {
variance_error = true;
} else {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if(ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
@ -931,11 +1120,12 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionActual to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle())
AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
}
/**
* @}

2
flight/modules/Autotune/autotune.c
View File

@ -120,7 +120,7 @@ MODULE_INITCALL(AutotuneInitialize, AutotuneStart)
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
static void AutotuneTask(__attribute__((unused)) void *parameters)
{
//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

2
flight/modules/Battery/battery.c
View File

@ -124,7 +124,7 @@ int32_t BatteryInitialize(void)
MODULE_INITCALL(BatteryInitialize, 0)
#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED
static void onTimer(UAVObjEvent* ev)
static void onTimer(__attribute__((unused)) UAVObjEvent* ev)
{
static FlightBatteryStateData flightBatteryData;
FlightBatterySettingsData batterySettings;

132
flight/modules/CallbackTest/callbacktest.c Normal file
View File

@ -0,0 +1,132 @@
/**
******************************************************************************
*
* @file callbacktest.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Example module to be used as a template for actual modules.
* Event callback version.
*
* @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
*/
/**
*
* This is a test suite to test the callback scheduler,
* including its forward scheduling ability
*
*/
#include "openpilot.h"
// Private constants
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
// Private types
//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__)
#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex);
static xSemaphoreHandle mutex;
// Private variables
static DelayedCallbackInfo *cbinfo[10];
static volatile int32_t counter[10]={0};
static void DelayedCb0();
static void DelayedCb1();
static void DelayedCb2();
static void DelayedCb3();
static void DelayedCb4();
static void DelayedCb5();
static void DelayedCb6();
/**
* Initialise the module, called on startup.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t CallbackTestInitialize()
{
mutex = xSemaphoreCreateRecursiveMutex();
cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE);
return 0;
}
int32_t CallbackTestStart() {
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
DelayedCallbackDispatch(cbinfo[3]);
DelayedCallbackDispatch(cbinfo[2]);
DelayedCallbackDispatch(cbinfo[1]);
DelayedCallbackDispatch(cbinfo[0]);
// different callback priorities within a taskpriority
DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE);
DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE);
DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER);
DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER);
DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER);
DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER);
DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE);
// should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
// delayed counter with 500 ms
DelayedCallbackDispatch(cbinfo[6]);
// high task prio
xSemaphoreGiveRecursive(mutex);
return 0;
}
static void DelayedCb0() {
DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]);
if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]);
}
static void DelayedCb1() {
DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]);
if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]);
}
static void DelayedCb2() {
DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]);
if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]);
}
static void DelayedCb3() {
DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]);
if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]);
}
static void DelayedCb4() {
DEBUGPRINT("delayed scheduled callback 4 reached!\n");
exit(0);
}
static void DelayedCb5() {
DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]);
if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
// it will likely only reach 8 due to cb4 aborting the run
}
static void DelayedCb6() {
DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]);
if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]);
}

4
flight/modules/CameraStab/camerastab.c
View File

@ -180,7 +180,7 @@ static void attitudeUpdated(UAVObjEvent* ev)
break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal * cameraStab.InputRate[i];
if (fabs(input_rate) > cameraStab.MaxAxisLockRate)
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate)
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]);
break;
default:
@ -328,7 +328,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
float delta = *attitude - csd->ffLastAttitudeFiltered[index];
float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis;
if (fabs(delta) > maxDelta) {
if (fabsf(delta) > maxDelta) {
// we are accelerating too hard
*attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta);
}

8
flight/modules/ComUsbBridge/ComUsbBridge.c
View File

@ -128,7 +128,7 @@ MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart)
* Main task. It does not return.
*/
static void com2UsbBridgeTask(void *parameters)
static void com2UsbBridgeTask(__attribute__((unused)) void *parameters)
{
/* Handle usart -> vcp direction */
volatile uint32_t tx_errors = 0;
@ -138,7 +138,7 @@ static void com2UsbBridgeTask(void *parameters)
rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != rx_bytes) {
if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) {
/* Error on transmit */
tx_errors++;
}
@ -146,7 +146,7 @@ static void com2UsbBridgeTask(void *parameters)
}
}
static void usb2ComBridgeTask(void * parameters)
static void usb2ComBridgeTask(__attribute__((unused)) void * parameters)
{
/* Handle vcp -> usart direction */
volatile uint32_t tx_errors = 0;
@ -156,7 +156,7 @@ static void usb2ComBridgeTask(void * parameters)
rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != rx_bytes) {
if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) {
/* Error on transmit */
tx_errors++;
}

140
flight/modules/Example/examplemodcallback.c Normal file
View File

@ -0,0 +1,140 @@
/**
******************************************************************************
*
* @file examplemodcallback.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Example module to be used as a template for actual modules.
* Event callback version.
*
* @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: ExampleObject1, ExampleSettings
* Output object: ExampleObject2
*
* This module executes in response to ExampleObject1 updates. When the
* module is triggered it will update the data of ExampleObject2.
*
* No threads are used in this example.
*
* 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 "openpilot.h"
#include "exampleobject1.h" // object the module will listen for updates (input)
#include "exampleobject2.h" // object the module will update (output)
#include "examplesettings.h" // object holding module settings (input)
// Private constants
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
// Private types
// Private variables
static DelayedCallbackInfo *cbinfo;
// Private functions
static void ObjectUpdatedCb(UAVObjEvent *ev);
static void DelayedCb();
/**
* Initialise the module, called on startup.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t ExampleModCallbackInitialize()
{
// Listen for ExampleObject1 updates, connect a callback function
ExampleObject1ConnectCallback(&ObjectUpdatedCb);
cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
return 0;
}
/**
* This function is called each time ExampleObject1 is updated, this could be
* a local update or a remote update from the GCS. In this example the module
* does not have its own thread, the callbacks are executed from within the
* event thread. Because of that the callback execution time must be kept to
* a minimum.
*/
static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(cbinfo);
}
/**
* This function is called by the DelayedCallbackScheduler when its execution
* has been requested. Callbacks scheduled for execution are executed in the
* same thread in a round robin fashion. The Dispatch function to reschedule
* execution can be called from within the Callback itself, in which case the
* re-run will be scheduled after all other callback with equal or higher
* priority have been executed. Like event callbacks, delayed callbacks are
* executed in the same thread context one at a time, therefore blocking IO
* functions or very long lasting calculations are prohibited. Unlike Event
* callbacks these callbacks run with a standard (IDLE+1) thread priority and
* do not block regular threads. They are therefore saver to use.
*/
static void DelayedCb();
ExampleSettingsData settings;
ExampleObject1Data data1;
ExampleObject2Data data2;
int32_t step;
// Update settings with latest value
ExampleSettingsGet(&settings);
// Get the input object
ExampleObject1Get(&data1);
// Determine how to update the output object
if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) {
step = settings.StepSize;
} else {
step = -settings.StepSize;
}
// Update data
data2.Field1 = data1.Field1 + step;
data2.Field2 = data1.Field2 + step;
data2.Field3 = data1.Field3 + step;
data2.Field4[0] = data1.Field4[0] + step;
data2.Field4[1] = data1.Field4[1] + step;
// Update the ExampleObject2, after this function is called
// notifications to any other modules listening to that object
// will be sent and the GCS object will be updated through the
// telemetry link. All operations will take place asynchronously
// and the following call will return immediately.
ExampleObject2Set(&data2);
//call the module again 10 seconds later,
//even if the exampleobject has not been updated
DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE);
}

2
flight/modules/Example/examplemodperiodic.c
View File

@ -76,7 +76,7 @@ int32_t ExampleModPeriodicInitialize()
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
static void exampleTask(__attribute__((unused)) void *parameters)
{
ExampleSettingsData settings;
ExampleObject2Data data;

2
flight/modules/Example/examplemodthread.c
View File

@ -84,7 +84,7 @@ int32_t ExampleModThreadInitialize()
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
static void exampleTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
ExampleSettingsData settings;

42
flight/modules/Extensions/MagBaro/magbaro.c
View File

@ -45,9 +45,9 @@
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 620
#define STACK_SIZE_BYTES 600
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define UPDATE_PERIOD 50
#define UPDATE_PERIOD 100
// Private types
@ -55,14 +55,20 @@
static xTaskHandle taskHandle;
// down sampling variables
static bool magbaroEnabled;
#if defined(PIOS_INCLUDE_BMP085)
#define alt_ds_size 4
static int32_t alt_ds_temp = 0;
static int32_t alt_ds_pres = 0;
static int alt_ds_count = 0;
#endif
#if defined(PIOS_INCLUDE_HMC5883)
int32_t mag_test;
static bool magbaroEnabled;
static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {1,1,1};
#endif
// Private functions
static void magbaroTask(void *parameters);
@ -77,7 +83,7 @@ int32_t MagBaroStart()
if (magbaroEnabled) {
// Start main task
xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle);
return 0;
}
return -1;
@ -104,13 +110,18 @@ int32_t MagBaroInitialize()
if(magbaroEnabled)
{
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerInitialize();
#endif
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeInitialize();
// init down-sampling data
alt_ds_temp = 0;
alt_ds_pres = 0;
alt_ds_count = 0;
#endif
}
return 0;
}
@ -118,7 +129,7 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart)
/**
* Module thread, should not return.
*/
#if defined(PIOS_INCLUDE_HMC5883)
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#ifdef PIOS_HMC5883_HAS_GPIOS
.exti_cfg = 0,
@ -129,27 +140,25 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
};
#endif
static void magbaroTask(void *parameters)
static void magbaroTask(__attribute__((unused)) void *parameters)
{
BaroAltitudeData data;
portTickType lastSysTime;
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeData data;
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
//mag_test = PIOS_HMC5883_Test();
#else
mag_test = 0;
MagnetometerData mag;
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
#endif
// Main task loop
lastSysTime = xTaskGetTickCount();
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1)
{
#if defined(PIOS_INCLUDE_BMP085)
@ -162,7 +171,7 @@ static void magbaroTask(void *parameters)
#endif
PIOS_BMP085_ReadADC();
alt_ds_temp += PIOS_BMP085_GetTemperature();
// Update the pressure data
PIOS_BMP085_StartADC(PressureConv);
#ifdef PIOS_BMP085_HAS_GPIOS
@ -172,7 +181,7 @@ static void magbaroTask(void *parameters)
#endif
PIOS_BMP085_ReadADC();
alt_ds_pres += PIOS_BMP085_GetPressure();
if (++alt_ds_count >= alt_ds_size)
{
alt_ds_count = 0;
@ -194,7 +203,6 @@ static void magbaroTask(void *parameters)
#endif
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);

2
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -238,7 +238,7 @@ static uint32_t get_time(void)
/**
* Executed by event dispatcher callback to reset INS before resetting OP
*/
static void resetTask(UAVObjEvent * ev)
static void resetTask(__attribute__((unused)) UAVObjEvent * ev)
{
#if defined (PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);

8
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -144,7 +144,7 @@ static float indicatedAirspeedActualBias = 0;
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
static void pathfollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
@ -316,7 +316,7 @@ static void updatePathVelocity()
if (angle1>180.0f) angle1-=360.0f;
if (angle2<-180.0f) angle2+=360.0f;
if (angle2>180.0f) angle2-=360.0f;
if (fabs(angle1)>=90.0f && fabs(angle2)>=90.0f) {
if (fabsf(angle1)>=90.0f && fabsf(angle2)>=90.0f) {
error_speed=0;
}
@ -630,13 +630,13 @@ static float bound(float val, float min, float max)
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
}
static void airspeedActualUpdatedCb(UAVObjEvent * ev)
static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedActualData airspeedActual;

2
flight/modules/FlightPlan/flightplan.c
View File

@ -97,7 +97,7 @@ MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart)
/**
* Module task
*/
static void flightPlanTask(void *parameters)
static void flightPlanTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
PmReturn_t retval;

28
flight/modules/FlightPlan/lib/uavobjecttemplate.pyt → flight/modules/FlightPlan/lib/uavobject.pyt.template
View File

@ -30,31 +30,21 @@
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#
from uavobject import *
$(DATAFIELDS)
# Object $(NAME) definition
class $(NAME)(UAVObject):
# Object constants
OBJID = $(UOBJID)
# Object constants
OBJID = $(UOBJID)
# Constructor
def __init__(self):
UAVObject.__init__(self, $(NAME).OBJID)
# Constructor
def __init__(self):
UAVObject.__init__(self, $(NAME).OBJID)
# Create object fields
# Create object fields
$(DATAFIELDINIT)
# Read field data
self.read()
self.metadata.read()
# Read field data
self.read()
self.metadata.read()

4
flight/modules/GPS/GPS.c
View File

@ -196,7 +196,7 @@ MODULE_INITCALL(GPSInitialize, GPSStart)
* Main gps task. It does not return.
*/
static void gpsTask(void *parameters)
static void gpsTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
@ -277,7 +277,7 @@ static void gpsTask(void *parameters)
/*
* Estimate the acceleration due to gravity for a particular location in LLA
*/
static float GravityAccel(float latitude, float longitude, float altitude)
static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude)
{
/* WGS84 gravity model. The effect of gravity over latitude is strong
* enough to change the estimated accelerometer bias in those apps. */

8
flight/modules/GPS/NMEA.c
View File

@ -78,7 +78,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
#endif //PIOS_GPS_MINIMAL
const static struct nmea_parser nmea_parsers[] = {
static const struct nmea_parser nmea_parsers[] = {
{
.prefix = "GPGGA",
.handler = nmeaProcessGPGGA,
@ -189,7 +189,7 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData,
return PARSER_INCOMPLETE;
}
const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
{
if (!prefix) {
return (NULL);
@ -601,7 +601,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
* \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
{
if (nbParam != 7)
return false;
@ -640,7 +640,7 @@ static uint8_t gsv_processed_mask;
static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error;
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
{
if (nbParam < 4)
return false;

2
flight/modules/MK/MKSerial/MKSerial.c
View File

@ -501,7 +501,7 @@ static void DoConnectedToNC(void)
}
}
static void MkSerialTask(void *parameters)
static void MkSerialTask(__attribute__((unused)) void *parameters)
{
MkMsg_t msg;
uint32_t version;

15
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -42,9 +42,12 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
(x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();
@ -105,8 +108,14 @@ int32_t ManualControlInitialize();
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int) FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int) FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int) FLIGHTSTATUS_FLIGHTMODE_LAND) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int) FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int) FLIGHTSTATUS_FLIGHTMODE_POI) \
)
#define assumptions_channelcount ( \

1520
flight/modules/ManualControl/manualcontrol.c
View File

@ -95,6 +95,7 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void updateLandDesired(ManualControlCommandData * cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
@ -115,10 +116,11 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
struct rcvr_activity_fsm
{
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
};
static struct rcvr_activity_fsm activity_fsm;
@ -132,12 +134,12 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
*/
int32_t ManualControlStart()
{
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
// Start main task
xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
return 0;
}
/**
@ -146,548 +148,552 @@ int32_t ManualControlStart()
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
/* Check the assumptions about uavobject enum's are correct */
if (!assumptions)
return -1;
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
return 0;
return 0;
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
/**
* Module task
*/
static void manualControlTask(void *parameters)
static void manualControlTask(__attribute__((unused)) void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
// Whenever the configuration changes, make sure it is safe to fly
// Run this initially to make sure the configuration is checked
configuration_check();
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Whenever the configuration changes, make sure it is safe to fly
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Read settings
ManualControlSettingsGet(&settings);
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
/* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
// Read settings
ManualControlSettingsGet(&settings);
if (!ManualControlCommandReadOnly()) {
/* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0;
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
++n) {
extern uint32_t pios_rcvr_group_map[];
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
settings.ChannelNumber[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
valid_input_detected = false;
else
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
if (!ManualControlCommandReadOnly()) {
// Check settings, if error raise alarm
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) && (
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
bool valid_input_detected = true;
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
// Read channel values in us
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
// immediately disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]);
}
continue;
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT)
valid_input_detected = false;
else
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Check settings, if error raise alarm
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
// immediately disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
continue;
}
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF
if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
processFlightMode(&settings, flightMode);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
processFlightMode(&settings, flightMode);
// Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
}
// Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
settings.ChannelMin,
settings.ChannelMax,
NELEMENTS(cmd.Channel));
}
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
settings.ChannelMin,
settings.ChannelMax,
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
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:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break;
}
lastFlightMode = flightStatus.FlightMode;
}
// Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
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:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break;
}
lastFlightMode = flightStatus.FlightMode;
}
}
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
{
ReceiverActivityData data;
bool updated = false;
ReceiverActivityData data;
bool updated = false;
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE &&
data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
}
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
}
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) {
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
}
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm)
{
bool activity_updated = false;
bool activity_updated = false;
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1;
channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP;
channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
ReceiverActivityActiveGroupSet((uint8_t*)&group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return (activity_updated);
ReceiverActivityActiveGroupSet((uint8_t*) &group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return (activity_updated);
}
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
{
bool activity_updated = false;
bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
fsm->prev,
NELEMENTS(fsm->prev));
fsm->sample_count++;
return (false);
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
return (false);
}
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
fsm->prev,
NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
return (activity_updated);
return (activity_updated);
}
static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(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 :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(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.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(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 :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(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 :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(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.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(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;
StabilizationDesiredSet(&stabilization);
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
#if defined(REVOLUTION)
@ -696,52 +702,92 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed,bool home)
{
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if(changed) {
// After not being in this mode for a while init at current height
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/*
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if(changed) {
// After not being in this mode for a while init at current height
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
*/
}
}
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime;
float dT;
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
}
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if(changed) {
// After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5;
PathDesiredSet(&pathDesired);
}
/**
@ -751,55 +797,66 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
*/
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{
const float DEADBAND_HIGH = 0.55f;
const float DEADBAND_LOW = 0.45f;
static portTickType lastSysTime;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
const float DEADBAND_HIGH = 0.55f;
const float DEADBAND_LOW = 0.45f;
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
static portTickType lastSysTime;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f);
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown;
PositionActualDownGet(&currentDown);
if(changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0;
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
else if (cmd->Throttle < DEADBAND_LOW && zeroed)
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) // Require the stick to enter the dead band before they can move height
zeroed = true;
AltitudeHoldDesiredSet(&altitudeHoldDesired);
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown;
PositionActualDownGet(&currentDown);
if(changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0;
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height
zeroed = true;
}
AltitudeHoldDesiredSet(&altitudeHoldDesired);
}
#else
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening
// in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed,
__attribute__((unused)) bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
#endif
/**
@ -807,38 +864,35 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral))
{
if (max != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral);
else
valueScaled = 0;
}
else
{
if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(neutral - min);
else
valueScaled = 0;
}
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) {
valueScaled = (float) (value - neutral) / (float) (max - neutral);
} else {
valueScaled = 0;
}
} else {
if (min != neutral) {
valueScaled = (float) (value - neutral) / (float) (neutral - min);
} else {
valueScaled = 0;
}
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled;
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
if(end_time > start_time)
return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
@ -847,24 +901,21 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
*/
static bool okToArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false;
}
}
return true;
return true;
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
@ -872,28 +923,29 @@ static bool okToArm(void)
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
static void setArmedIfChanged(uint8_t val)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
if (flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
@ -904,116 +956,121 @@ static void setArmedIfChanged(uint8_t val) {
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
bool lowThrottle = cmd->Throttle <= 0;
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
lowThrottle = true;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
lowThrottle = true;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch (armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
case ARMING_CHANNEL_ROLL:
armingInputLevel = sign * cmd->Roll;
break;
case ARMING_CHANNEL_PITCH:
armingInputLevel = sign * cmd->Pitch;
break;
case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw;
break;
}
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch (armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
@ -1024,20 +1081,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
*/
static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber)
pos = settings->FlightModeNumber - 1;
// Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber)
pos = settings->FlightModeNumber - 1;
uint8_t newMode = settings->FlightModePosition[pos];
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
@ -1046,13 +1103,12 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max)
{
int16_t tmp = min;
min = max;
max = tmp;
}
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
if (min > max) {
int16_t tmp = min;
min = max;
max = tmp;
}
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
/**
@ -1060,13 +1116,12 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
*/
static void applyDeadband(float *value, float deadband)
{
if (fabs(*value) < deadband)
*value = 0.0f;
else
if (*value > 0.0f)
*value -= deadband;
else
*value += deadband;
if (fabsf(*value) < deadband)
*value = 0.0f;
else if (*value > 0.0f)
*value -= deadband;
else
*value += deadband;
}
#ifdef USE_INPUT_LPF
@ -1075,25 +1130,22 @@ static void applyDeadband(float *value, float deadband)
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (settings->ResponseTime[channel]) {
float rt = (float)settings->ResponseTime[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
if (settings->ResponseTime[channel]) {
float rt = (float)settings->ResponseTime[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
}
#endif // USE_INPUT_LPF
/**
* Called whenever a critical configuration component changes
*/
static void configurationUpdatedCb(UAVObjEvent * ev)
static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
configuration_check();
configuration_check();
}
/**
* @}
* @}
*/
* @}
* @}
*/

5
flight/modules/OPLink/oplinkmod.c
View File

@ -123,7 +123,7 @@ MODULE_INITCALL(OPLinkModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
static void systemTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
uint16_t prev_tx_count = 0;
@ -227,7 +227,8 @@ void vApplicationIdleHook(void)
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask,
__attribute__((unused)) signed portCHAR * pcTaskName)
{
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW

10
flight/modules/Osd/OsdEtStd/OsdEtStd.c
View File

@ -41,7 +41,7 @@
//
#define DEBUG_PORT PIOS_COM_GPS
//#define ENABLE_DEBUG_MSG
//#define USE_DEBUG_PINS
//#define PIOS_ENABLE_DEBUG_PINS
//#define DUMP_CONFIG // Enable this do read and dump the OSD config
//
@ -78,7 +78,7 @@
#define OSDMSG_GPS_STAT_FIX 0x2B
#define OSDMSG_GPS_STAT_HB_FLAG 0x10
#ifdef USE_DEBUG_PINS
#ifdef PIOS_ENABLE_DEBUG_PINS
#define DEBUG_PIN_RUNNING 0
#define DEBUG_PIN_I2C 1
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
@ -206,17 +206,17 @@ static void SetNbSats(uint8_t nb)
msg[OSDMSG_NB_SATS] = nb;
}
static void FlightBatteryStateUpdatedCb(UAVObjEvent * ev)
static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newBattData = TRUE;
}
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newPosData = TRUE;
}
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newBaroData = TRUE;
}

38
flight/modules/Osd/WavPlayer/inc/wavplayer.h
View File

@ -1,15 +1,37 @@
/*
* WavPlayer.h
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup WavPlayerModule WavPlayer Module
* @brief Process WavPlayer information
* @{
*
* Created on: 15.07.2012
* Author: Samba
* @file wavplayer.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief WavPlayer 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 WavPlayer_H_
#define WavPlayer_H_
#ifndef WAVPLAYER_H_
#define WAVPLAYER_H_
#include "openpilot.h"
int32_t WavPlayerInitialize(void);
#endif /* WavPlayer_H_ */
#endif /* WAVPLAYER_H_ */

65
flight/modules/Osd/WavPlayer/wavplayer.c
View File

@ -6,7 +6,7 @@
* @brief Process WavPlayer information
* @{
*
* @file WavPlayer.c
* @file wavplayer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief WavPlayer module
* @see The GNU Public License (GPL) Version 3
@ -29,10 +29,8 @@
*/
// ****************
#include "openpilot.h"
// ****************
// Private functions
@ -56,10 +54,10 @@ static uint32_t timeOfLastUpdateMs;
// ****************
int32_t WavPlayerStart(void)
{
// Start WavPlayer task
xTaskCreate(WavPlayerTask, (signed char *)"WavPlayer", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle);
// Start WavPlayer task
xTaskCreate(WavPlayerTask, (signed char *) "WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle);
return 0;
return 0;
}
/**
* Initialise the WavPlayer module
@ -69,54 +67,37 @@ int32_t WavPlayerStart(void)
int32_t WavPlayerInitialize(void)
{
return 0;
return 0;
}
MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart)
MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart)
// ****************
/**
* Main gps task. It does not return.
* Main WavPlayer task. It does not return.
*/
static void WavPlayerTask(void *parameters)
static void WavPlayerTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
// Loop forever
lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
portTickType lastSysTime;
// Loop forever
lastSysTime = xTaskGetTickCount();
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
#if defined(PIOS_INCLUDE_WAVE)
WavePlayer_Start();
WavePlayer_Start();
#endif
// Loop forever
while (1)
{
vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
/*if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
{ // we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
}
else
{ // we appear to be receiving GPS sentences OK, we've had an update
}*/
}
// Loop forever
while (1) {
vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

59
flight/modules/Osd/osdgen/inc/osdgen.h
View File

@ -1,18 +1,41 @@
/*
* osdgen.h
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup OSDgenModule osdgen Module
* @brief Process OSD information
* @{
*
* Created on: 2.10.2011
* Author: Samba
* @file osdgen.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief OSD gen module, handles OSD draw. Parts from CL-OSD and SUPEROSD projects
* @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 OSDGEN_H_
#define OSDGEN_H_
#include "openpilot.h"
#include "pios.h"
int32_t osdgenInitialize(void);
// Size of an array (num items.)
#define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0]))
@ -78,12 +101,9 @@ int32_t osdgenInitialize(void);
write_pixel(buff, (cx) + (x), (cy) - (y), mode); \
write_pixel(buff, (cx) - (x), (cy) - (y), mode);
// Font flags.
#define FONT_BOLD 1 // bold text (no outline)
#define FONT_INVERT 2 // invert: border white, inside black
// Text alignments.
#define TEXT_VA_TOP 0
#define TEXT_VA_MIDDLE 1
@ -95,10 +115,9 @@ int32_t osdgenInitialize(void);
// Text dimension structures.
struct FontDimensions
{
int width, height;
int width, height;
};
// Max/Min macros.
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
@ -110,20 +129,19 @@ struct FontDimensions
#define APPLY_VDEADBAND(y) ((y)+GRAPHICS_VDEADBAND)
#define APPLY_HDEADBAND(x) ((x)+GRAPHICS_HDEADBAND)
// Check if coordinates are valid. If not, return.
#define CHECK_COORDS(x, y) if(x < 0 || x >= GRAPHICS_WIDTH_REAL || y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
#define CHECK_COORD_X(x) if(x < 0 || x >= GRAPHICS_WIDTH_REAL) return;
#define CHECK_COORD_Y(y) if(y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
// Check if coordinates are valid. If not, return. Assumes unsigned coordinate
#define CHECK_COORDS(x, y) if(x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) return;
#define CHECK_COORD_X(x) if(x >= GRAPHICS_WIDTH_REAL) return;
#define CHECK_COORD_Y(y) if(y >= GRAPHICS_HEIGHT_REAL) return;
// Clip coordinates out of range.
#define CLIP_COORD_X(x) { x = MAX(0, MIN(x, GRAPHICS_WIDTH_REAL)); }
#define CLIP_COORD_Y(y) { y = MAX(0, MIN(y, GRAPHICS_HEIGHT_REAL)); }
// Clip coordinates out of range - assumes unsigned coordinate
#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); }
#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); }
#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); }
// Macro to swap two variables using XOR swap.
#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; }
// Line triggering
#define LAST_LINE 312 //625/2 //PAL
//#define LAST_LINE 525/2 //NTSC
@ -141,8 +159,6 @@ struct FontDimensions
#define DELAY_9_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n")
#define DELAY_10_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n")
uint8_t getCharData(uint16_t charPos);
void introText();
@ -189,7 +205,4 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
void updateOnceEveryFrame();
#endif /* OSDGEN_H_ */

3354
flight/modules/Osd/osdgen/osdgen.c
View File

@ -2,7 +2,7 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup OSDGENModule osdgen Module
* @addtogroup OSDgenModule osdgen Module
* @brief Process OSD information
* @{
*
@ -42,6 +42,7 @@
#include "osdsettings.h"
#include "baroaltitude.h"
#include "taskinfo.h"
#include "flightstatus.h"
#include "fonts.h"
#include "font12x18.h"
@ -50,29 +51,28 @@
#include "splash.h"
/*
static uint16_t angleA=0;
static int16_t angleB=90;
static int16_t angleC=0;
static int16_t sum=2;
static uint16_t angleA=0;
static int16_t angleB=90;
static int16_t angleC=0;
static int16_t sum=2;
static int16_t m_pitch=0;
static int16_t m_roll=0;
static int16_t m_yaw=0;
static int16_t m_batt=0;
static int16_t m_alt=0;
static int16_t m_pitch=0;
static int16_t m_roll=0;
static int16_t m_yaw=0;
static int16_t m_batt=0;
static int16_t m_alt=0;
static uint8_t m_gpsStatus=0;
static int32_t m_gpsLat=0;
static int32_t m_gpsLon=0;
static float m_gpsAlt=0;
static float m_gpsSpd=0;*/
static uint8_t m_gpsStatus=0;
static int32_t m_gpsLat=0;
static int32_t m_gpsLon=0;
static float m_gpsAlt=0;
static float m_gpsSpd=0;*/
extern uint8_t *draw_buffer_level;
extern uint8_t *draw_buffer_mask;
extern uint8_t *disp_buffer_level;
extern uint8_t *disp_buffer_mask;
TTime timex;
// ****************
@ -97,9 +97,9 @@ static xTaskHandle osdgenTaskHandle;
struct splashEntry
{
unsigned int width, height;
const uint16_t *level;
const uint16_t *mask;
unsigned int width, height;
const uint16_t *level;
const uint16_t *mask;
};
struct splashEntry splash[3] = {
@ -114,126 +114,126 @@ struct splashEntry splash[3] = {
{ llama_width,
llama_height,
llama_bits,
llama_mask_bits },
llama_mask_bits }
};
uint16_t mirror(uint16_t source)
{
int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) |
((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) |
((source & 0x0800) << 1) | ((source & 0x0400) << 3) |
((source & 0x0200) << 5) | ((source & 0x0100) << 7) |
((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) |
((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) |
((source & 0x0008) << 1) | ((source & 0x0004) << 3) |
((source & 0x0002) << 5) | ((source & 0x0001) << 7);
int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1)
| ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5)
| ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5)
| ((source & 0x0001) << 7);
return result;
return result;
}
void clearGraphics() {
memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
void clearGraphics()
{
memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
}
void copyimage(uint16_t offsetx, uint16_t offsety, int image) {
//check top/left position
if (!validPos(offsetx, offsety)) {
return;
}
struct splashEntry splash_info;
splash_info = splash[image];
offsetx=offsetx/8;
for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) {
uint16_t x1=offsetx;
for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) {
draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
draw_buffer_mask[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
x1+=2;
}
}
void copyimage(uint16_t offsetx, uint16_t offsety, int image)
{
//check top/left position
if (!validPos(offsetx, offsety)) {
return;
}
struct splashEntry splash_info;
splash_info = splash[image];
offsetx = offsetx / 8;
for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
uint16_t x1 = offsetx;
for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
x1 += 2;
}
}
}
uint8_t validPos(uint16_t x, uint16_t y) {
if ( x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
return 0;
}
return 1;
uint8_t validPos(uint16_t x, uint16_t y)
{
if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
return 0;
}
return 1;
}
// Credit for this one goes to wikipedia! :-)
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
int f = 1 - radius;
int ddF_x = 1;
int ddF_y = -2 * radius;
int x = 0;
int y = radius;
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius)
{
int f = 1 - radius;
int ddF_x = 1;
int ddF_y = -2 * radius;
int x = 0;
int y = radius;
write_pixel_lm(x0, y0 + radius,1,1);
write_pixel_lm(x0, y0 - radius,1,1);
write_pixel_lm(x0 + radius, y0,1,1);
write_pixel_lm(x0 - radius, y0,1,1);
write_pixel_lm(x0, y0 + radius, 1, 1);
write_pixel_lm(x0, y0 - radius, 1, 1);
write_pixel_lm(x0 + radius, y0, 1, 1);
write_pixel_lm(x0 - radius, y0, 1, 1);
while(x < y)
{
// ddF_x == 2 * x + 1;
// ddF_y == -2 * y;
// f == x*x + y*y - radius*radius + 2*x - y + 1;
if(f >= 0)
{
y--;
ddF_y += 2;
f += ddF_y;
}
x++;
ddF_x += 2;
f += ddF_x;
write_pixel_lm(x0 + x, y0 + y,1,1);
write_pixel_lm(x0 - x, y0 + y,1,1);
write_pixel_lm(x0 + x, y0 - y,1,1);
write_pixel_lm(x0 - x, y0 - y,1,1);
write_pixel_lm(x0 + y, y0 + x,1,1);
write_pixel_lm(x0 - y, y0 + x,1,1);
write_pixel_lm(x0 + y, y0 - x,1,1);
write_pixel_lm(x0 - y, y0 - x,1,1);
}
while (x < y) {
// ddF_x == 2 * x + 1;
// ddF_y == -2 * y;
// f == x*x + y*y - radius*radius + 2*x - y + 1;
if (f >= 0) {
y--;
ddF_y += 2;
f += ddF_y;
}
x++;
ddF_x += 2;
f += ddF_x;
write_pixel_lm(x0 + x, y0 + y, 1, 1);
write_pixel_lm(x0 - x, y0 + y, 1, 1);
write_pixel_lm(x0 + x, y0 - y, 1, 1);
write_pixel_lm(x0 - x, y0 - y, 1, 1);
write_pixel_lm(x0 + y, y0 + x, 1, 1);
write_pixel_lm(x0 - y, y0 + x, 1, 1);
write_pixel_lm(x0 + y, y0 - x, 1, 1);
write_pixel_lm(x0 - y, y0 - x, 1, 1);
}
}
void swap(uint16_t* a, uint16_t* b) {
uint16_t temp = *a;
*a = *b;
*b = temp;
void swap(uint16_t* a, uint16_t* b)
{
uint16_t temp = *a;
*a = *b;
*b = temp;
}
static const int8_t sinData[91] =
{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };
const static int8_t sinData[91] = {
0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33,
34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62,
63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84,
85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97,
97, 98, 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100};
static int8_t mySin(uint16_t angle) {
uint16_t pos = 0;
pos = angle % 360;
int8_t mult = 1;
// 180-359 is same as 0-179 but negative.
if (pos >= 180) {
pos = pos - 180;
mult = -1;
}
// 0-89 is equal to 90-179 except backwards.
if (pos >= 90) {
pos = 180 - pos;
}
return mult * (int8_t)(sinData[pos]);
static int8_t mySin(uint16_t angle)
{
uint16_t pos = 0;
pos = angle % 360;
int8_t mult = 1;
// 180-359 is same as 0-179 but negative.
if (pos >= 180) {
pos = pos - 180;
mult = -1;
}
// 0-89 is equal to 90-179 except backwards.
if (pos >= 90) {
pos = 180 - pos;
}
return mult * (int8_t)(sinData[pos]);
}
static int8_t myCos(uint16_t angle) {
return mySin(angle + 90);
static int8_t myCos(uint16_t angle)
{
return mySin(angle + 90);
}
/// Draws four points relative to the given center point.
@ -250,10 +250,10 @@ static int8_t myCos(uint16_t angle) {
/// \param color the color to draw the pixels with.
void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY)
{
write_pixel_lm(centerX + deltaX, centerY + deltaY,1,1); // Ist Quadrant
write_pixel_lm(centerX - deltaX, centerY + deltaY,1,1); // IInd Quadrant
write_pixel_lm(centerX - deltaX, centerY - deltaY,1,1); // IIIrd Quadrant
write_pixel_lm(centerX + deltaX, centerY - deltaY,1,1); // IVth Quadrant
write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant
write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant
write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant
write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant
}
/// Implements the midpoint ellipse drawing algorithm which is a bresenham
@ -278,62 +278,57 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
plotFourQuadrants(centerX, centerY, x, y);
while(deltaY >= deltaX)
{
x++;
deltaX += (doubleVerticalRadius << 1);
while (deltaY >= deltaX) {
x++;
deltaX += (doubleVerticalRadius << 1);
error += deltaX + doubleVerticalRadius;
error += deltaX + doubleVerticalRadius;
if(error >= 0)
{
y--;
deltaY -= (doubleHorizontalRadius << 1);
if (error >= 0) {
y--;
deltaY -= (doubleHorizontalRadius << 1);
error -= deltaY;
}
plotFourQuadrants(centerX, centerY, x, y);
error -= deltaY;
}
plotFourQuadrants(centerX, centerY, x, y);
}
error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
while (y>=0)
{
error += doubleHorizontalRadius;
y--;
deltaY -= (doubleHorizontalRadius<<1);
error -= deltaY;
while (y >= 0) {
error += doubleHorizontalRadius;
y--;
deltaY -= (doubleHorizontalRadius << 1);
error -= deltaY;
if(error <= 0)
{
x++;
deltaX += (doubleVerticalRadius << 1);
error += deltaX;
}
if (error <= 0) {
x++;
deltaX += (doubleVerticalRadius << 1);
error += deltaX;
}
plotFourQuadrants(centerX, centerY, x, y);
plotFourQuadrants(centerX, centerY, x, y);
}
}
void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size)
{
int16_t a = myCos(angle);
int16_t b = mySin(angle);
a = (a * (size/2)) / 100;
b = (b * (size/2)) / 100;
write_line_lm((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a, 1, 1); //Direction line
//write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 - a/2, (y)-1 - b/2, 1, 1); // Arrow "wings"
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 + a/2, (y)-1 + b/2, 1, 1);
int16_t a = myCos(angle);
int16_t b = mySin(angle);
a = (a * (size / 2)) / 100;
b = (b * (size / 2)) / 100;
write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line
//write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings"
write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1);
}
void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
write_line_lm(x1, y1, x2, y1, 1, 1); //top
write_line_lm(x1, y1, x1, y2, 1, 1); //left
write_line_lm(x2, y1, x2, y2, 1, 1); //right
write_line_lm(x1, y2, x2, y2, 1, 1); //bottom
write_line_lm(x1, y1, x2, y1, 1, 1); //top
write_line_lm(x1, y1, x1, y2, 1, 1); //left
write_line_lm(x2, y1, x2, y2, 1, 1); //right
write_line_lm(x1, y2, x2, y2, 1, 1); //bottom
}
// simple routines
@ -350,14 +345,14 @@ void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
*/
void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
{
CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word
// index to set it in.
int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y);
// Apply a mask.
uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(buff, wordnum, mask, mode);
CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word
// index to set it in.
int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y);
// Apply a mask.
uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(buff, wordnum, mask, mode);
}
/**
@ -371,18 +366,17 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
*/
void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
{
CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word
// index to set it in.
int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y);
// Apply the masks.
uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word
// index to set it in.
int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y);
// Apply the masks.
uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
}
/**
* write_hline: optimised horizontal line writing algorithm
*
@ -394,41 +388,38 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
*/
void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode)
{
CLIP_COORDS(x0, y);
CLIP_COORDS(x1, y);
if(x0 > x1)
{
SWAP(x0, x1);
}
if(x0 == x1) return;
/* This is an optimised algorithm for writing horizontal lines.
* We begin by finding the addresses of the x0 and x1 points. */
int addr0 = CALC_BUFF_ADDR(x0, y);
int addr1 = CALC_BUFF_ADDR(x1, y);
int addr0_bit = CALC_BIT_IN_WORD(x0);
int addr1_bit = CALC_BIT_IN_WORD(x1);
int mask, mask_l, mask_r, i;
/* If the addresses are equal, we only need to write one word
* which is an island. */
if(addr0 == addr1)
{
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask, mode);
}
/* Otherwise we need to write the edges and then the middle. */
else
{
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
// Now write 0xffff words from start+1 to end-1.
for(i = addr0 + 1; i <= addr1 - 1; i++)
{
uint8_t m=0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
}
CLIP_COORDS(x0, y);
CLIP_COORDS(x1, y);
if (x0 > x1) {
SWAP(x0, x1);
}
if (x0 == x1) {
return;
}
/* This is an optimised algorithm for writing horizontal lines.
* We begin by finding the addresses of the x0 and x1 points. */
int addr0 = CALC_BUFF_ADDR(x0, y);
int addr1 = CALC_BUFF_ADDR(x1, y);
int addr0_bit = CALC_BIT_IN_WORD(x0);
int addr1_bit = CALC_BIT_IN_WORD(x1);
int mask, mask_l, mask_r, i;
/* If the addresses are equal, we only need to write one word
* which is an island. */
if (addr0 == addr1) {
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask, mode);
} else {
/* Otherwise we need to write the edges and then the middle. */
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
// Now write 0xffff words from start+1 to end-1.
for (i = addr0 + 1; i <= addr1 - 1; i++) {
uint8_t m = 0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
}
}
/**
@ -442,10 +433,10 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
*/
void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode)
{
// TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously.
write_hline(draw_buffer_level, x0, x1, y, lmode);
write_hline(draw_buffer_mask, x0, x1, y, mmode);
// TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously.
write_hline(draw_buffer_level, x0, x1, y, lmode);
write_hline(draw_buffer_mask, x0, x1, y, mmode);
}
/**
@ -462,19 +453,18 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode,
*/
void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode)
{
int stroke, fill;
SETUP_STROKE_FILL(stroke, fill, mode)
if(x0 > x1)
{
SWAP(x0, x1);
}
// Draw the main body of the line.
write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
// Draw the endcaps, if any.
DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
int stroke, fill;
SETUP_STROKE_FILL(stroke, fill, mode)
if (x0 > x1) {
SWAP(x0, x1);
}
// Draw the main body of the line.
write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
// Draw the endcaps, if any.
DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
}
/**
@ -488,27 +478,27 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
*/
void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode)
{
unsigned int a;
CLIP_COORDS(x, y0);
CLIP_COORDS(x, y1);
if(y0 > y1)
{
SWAP(y0, y1);
}
if(y0 == y1) return;
/* This is an optimised algorithm for writing vertical lines.
* We begin by finding the addresses of the x,y0 and x,y1 points. */
int addr0 = CALC_BUFF_ADDR(x, y0);
int addr1 = CALC_BUFF_ADDR(x, y1);
/* Then we calculate the pixel data to be written. */
int bitnum = CALC_BIT_IN_WORD(x);
uint16_t mask = 1 << (7 - bitnum);
/* Run from addr0 to addr1 placing pixels. Increment by the number
* of words n each graphics line. */
for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8)
{
WRITE_WORD_MODE(buff, a, mask, mode);
}
unsigned int a;
CLIP_COORDS(x, y0);
CLIP_COORDS(x, y1);
if (y0 > y1) {
SWAP(y0, y1);
}
if (y0 == y1) {
return;
}
/* This is an optimised algorithm for writing vertical lines.
* We begin by finding the addresses of the x,y0 and x,y1 points. */
unsigned int addr0 = CALC_BUFF_ADDR(x, y0);
unsigned int addr1 = CALC_BUFF_ADDR(x, y1);
/* Then we calculate the pixel data to be written. */
unsigned int bitnum = CALC_BIT_IN_WORD(x);
uint16_t mask = 1 << (7 - bitnum);
/* Run from addr0 to addr1 placing pixels. Increment by the number
* of words n each graphics line. */
for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
WRITE_WORD_MODE(buff, a, mask, mode);
}
}
/**
@ -522,10 +512,10 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
*/
void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode)
{
// TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously.
write_vline(draw_buffer_level, x, y0, y1, lmode);
write_vline(draw_buffer_mask, x, y0, y1, mmode);
// TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously.
write_vline(draw_buffer_level, x, y0, y1, lmode);
write_vline(draw_buffer_mask, x, y0, y1, mmode);
}
/**
@ -542,19 +532,18 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode,
*/
void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
{
int stroke, fill;
if(y0 > y1)
{
SWAP(y0, y1);
}
SETUP_STROKE_FILL(stroke, fill, mode);
// Draw the main body of the line.
write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
// Draw the endcaps, if any.
DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
int stroke, fill;
if (y0 > y1) {
SWAP(y0, y1);
}
SETUP_STROKE_FILL(stroke, fill, mode);
// Draw the main body of the line.
write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
// Draw the endcaps, if any.
DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
}
/**
@ -573,61 +562,56 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int
*/
void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode)
{
int yy, addr0_old, addr1_old;
CHECK_COORDS(x, y);
CHECK_COORD_X(x + width);
CHECK_COORD_Y(y + height);
if(width <= 0 || height <= 0) return;
// Calculate as if the rectangle was only a horizontal line. We then
// step these addresses through each row until we iterate `height` times.
int addr0 = CALC_BUFF_ADDR(x, y);
int addr1 = CALC_BUFF_ADDR(x + width, y);
int addr0_bit = CALC_BIT_IN_WORD(x);
int addr1_bit = CALC_BIT_IN_WORD(x + width);
int mask, mask_l, mask_r, i;
// If the addresses are equal, we need to write one word vertically.
if(addr0 == addr1)
{
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
while(height--)
{
WRITE_WORD_MODE(buff, addr0, mask, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8;
}
unsigned int yy, addr0_old, addr1_old;
CHECK_COORDS(x, y);
CHECK_COORD_X(x + width);
CHECK_COORD_Y(y + height);
if (width <= 0 || height <= 0) {
return;
}
// Calculate as if the rectangle was only a horizontal line. We then
// step these addresses through each row until we iterate `height` times.
unsigned int addr0 = CALC_BUFF_ADDR(x, y);
unsigned int addr1 = CALC_BUFF_ADDR(x + width, y);
unsigned int addr0_bit = CALC_BIT_IN_WORD(x);
unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width);
unsigned int mask, mask_l, mask_r, i;
// If the addresses are equal, we need to write one word vertically.
if (addr0 == addr1) {
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
while (height--) {
WRITE_WORD_MODE(buff, addr0, mask, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8;
}
} else {
// Otherwise we need to write the edges and then the middle repeatedly.
else
{
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
// Write edges first.
yy = 0;
addr0_old = addr0;
addr1_old = addr1;
while(yy < height)
{
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
// Now write 0xffff words from start+1 to end-1 for each row.
yy = 0;
addr0 = addr0_old;
addr1 = addr1_old;
while(yy < height)
{
for(i = addr0 + 1; i <= addr1 - 1; i++)
{
uint8_t m=0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
// Write edges first.
yy = 0;
addr0_old = addr0;
addr1_old = addr1;
while (yy < height) {
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
// Now write 0xffff words from start+1 to end-1 for each row.
yy = 0;
addr0 = addr0_old;
addr1 = addr1_old;
while (yy < height) {
for (i = addr0 + 1; i <= addr1 - 1; i++) {
uint8_t m = 0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
}
}
/**
@ -642,8 +626,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
*/
void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode)
{
write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
}
/**
@ -659,14 +643,14 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt
*/
void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode)
{
//CHECK_COORDS(x, y);
//CHECK_COORDS(x + width, y + height);
//if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
//if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
//CHECK_COORDS(x, y);
//CHECK_COORDS(x + width, y + height);
//if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
//if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
}
/**
@ -682,22 +666,19 @@ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int hei
*/
void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int mode)
{
CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0;
while(x >= y)
{
if(dashp == 0 || (y % dashp) < (dashp / 2))
{
CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
}
error += (y * 2) + 1;
y++;
if(error >= 0)
{
--x;
error -= x * 2;
}
}
CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0;
while (x >= y) {
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
}
error += (y * 2) + 1;
y++;
if (error >= 0) {
--x;
error -= x * 2;
}
}
}
/**
@ -713,58 +694,51 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
*/
void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode)
{
int stroke, fill;
CHECK_COORDS(cx, cy);
SETUP_STROKE_FILL(stroke, fill, mode);
// This is a two step procedure. First, we draw the outline of the
// circle, then we draw the inner part.
int error = -r, x = r, y = 0;
while(x >= y)
{
if(dashp == 0 || (y % dashp) < (dashp / 2))
{
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
if(bmode == 1)
{
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
}
}
error += (y * 2) + 1;
y++;
if(error >= 0)
{
--x;
error -= x * 2;
}
}
error = -r;
x = r;
y = 0;
while(x >= y)
{
if(dashp == 0 || (y % dashp) < (dashp / 2))
{
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
}
error += (y * 2) + 1;
y++;
if(error >= 0)
{
--x;
error -= x * 2;
}
}
int stroke, fill;
CHECK_COORDS(cx, cy);
SETUP_STROKE_FILL(stroke, fill, mode);
// This is a two step procedure. First, we draw the outline of the
// circle, then we draw the inner part.
int error = -r, x = r, y = 0;
while (x >= y) {
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
if (bmode == 1) {
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
}
}
error += (y * 2) + 1;
y++;
if (error >= 0) {
--x;
error -= x * 2;
}
}
error = -r;
x = r;
y = 0;
while (x >= y) {
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
}
error += (y * 2) + 1;
y++;
if (error >= 0) {
--x;
error -= x * 2;
}
}
}
/**
@ -778,42 +752,37 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
*/
void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, int mode)
{
CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0, xch = 0;
// It turns out that filled circles can take advantage of the midpoint
// circle algorithm. We simply draw very fast horizontal lines across each
// pair of X,Y coordinates. In some cases, this can even be faster than
// drawing an outlined circle!
//
// Due to multiple writes to each set of pixels, we have a special exception
// for when using the toggling draw mode.
while(x >= y)
{
if(y != 0)
{
write_hline(buff, cx - x, cx + x, cy + y, mode);
write_hline(buff, cx - x, cx + x, cy - y, mode);
if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y)))
{
write_hline(buff, cx - y, cx + y, cy + x, mode);
write_hline(buff, cx - y, cx + y, cy - x, mode);
xch = 0;
}
}
error += (y * 2) + 1;
y++;
if(error >= 0)
{
--x;
xch = 1;
error -= x * 2;
}
}
// Handle toggle mode.
if(mode == 2)
{
write_hline(buff, cx - r, cx + r, cy, mode);
}
CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0, xch = 0;
// It turns out that filled circles can take advantage of the midpoint
// circle algorithm. We simply draw very fast horizontal lines across each
// pair of X,Y coordinates. In some cases, this can even be faster than
// drawing an outlined circle!
//
// Due to multiple writes to each set of pixels, we have a special exception
// for when using the toggling draw mode.
while (x >= y) {
if (y != 0) {
write_hline(buff, cx - x, cx + x, cy + y, mode);
write_hline(buff, cx - x, cx + x, cy - y, mode);
if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
write_hline(buff, cx - y, cx + y, cy + x, mode);
write_hline(buff, cx - y, cx + y, cy - x, mode);
xch = 0;
}
}
error += (y * 2) + 1;
y++;
if (error >= 0) {
--x;
xch = 1;
error -= x * 2;
}
}
// Handle toggle mode.
if (mode == 2) {
write_hline(buff, cx - r, cx + r, cy, mode);
}
}
/**
@ -828,45 +797,39 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
*/
void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode)
{
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
int steep = abs(y1 - y0) > abs(x1 - x0);
if(steep)
{
SWAP(x0, y0);
SWAP(x1, y1);
}
if(x0 > x1)
{
SWAP(x0, x1);
SWAP(y0, y1);
}
int deltax = x1 - x0;
int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
int y = y0;
int x; //, lasty = y, stox = 0;
if(y0 < y1)
ystep = 1;
else
ystep = -1;
for(x = x0; x < x1; x++)
{
if(steep)
{
write_pixel(buff, y, x, mode);
}
else
{
write_pixel(buff, x, y, mode);
}
error -= deltay;
if(error < 0)
{
y += ystep;
error += deltax;
}
}
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
unsigned int steep = abs(y1 - y0) > abs(x1 - x0);
if (steep) {
SWAP(x0, y0);
SWAP(x1, y1);
}
if (x0 > x1) {
SWAP(x0, x1);
SWAP(y0, y1);
}
int deltax = x1 - x0;
unsigned int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
unsigned int y = y0;
unsigned int x; //, lasty = y, stox = 0;
if (y0 < y1) {
ystep = 1;
} else {
ystep = -1;
}
for (x = x0; x < x1; x++) {
if (steep) {
write_pixel(buff, y, x, mode);
} else {
write_pixel(buff, x, y, mode);
}
error -= deltay;
if (error < 0) {
y += ystep;
error += deltax;
}
}
}
/**
@ -881,8 +844,8 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
*/
void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode)
{
write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
}
/**
@ -898,86 +861,74 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i
* @param mode 0 = black outline, white body, 1 = white outline, black body
* @param mmode 0 = clear, 1 = set, 2 = toggle
*/
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
__attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1,
int mode, int mmode)
{
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
// This could be improved for speed.
int omode, imode;
if(mode == 0)
{
omode = 0;
imode = 1;
}
else
{
omode = 1;
imode = 0;
}
int steep = abs(y1 - y0) > abs(x1 - x0);
if(steep)
{
SWAP(x0, y0);
SWAP(x1, y1);
}
if(x0 > x1)
{
SWAP(x0, x1);
SWAP(y0, y1);
}
int deltax = x1 - x0;
int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
int y = y0;
int x;
if(y0 < y1)
ystep = 1;
else
ystep = -1;
// Draw the outline.
for(x = x0; x < x1; x++)
{
if(steep)
{
write_pixel_lm(y - 1, x, mmode, omode);
write_pixel_lm(y + 1, x, mmode, omode);
write_pixel_lm(y, x - 1, mmode, omode);
write_pixel_lm(y, x + 1, mmode, omode);
}
else
{
write_pixel_lm(x - 1, y, mmode, omode);
write_pixel_lm(x + 1, y, mmode, omode);
write_pixel_lm(x, y - 1, mmode, omode);
write_pixel_lm(x, y + 1, mmode, omode);
}
error -= deltay;
if(error < 0)
{
y += ystep;
error += deltax;
}
}
// Now draw the innards.
error = deltax / 2;
y = y0;
for(x = x0; x < x1; x++)
{
if(steep)
{
write_pixel_lm(y, x, mmode, imode);
}
else
{
write_pixel_lm(x, y, mmode, imode);
}
error -= deltay;
if(error < 0)
{
y += ystep;
error += deltax;
}
}
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
// This could be improved for speed.
int omode, imode;
if (mode == 0) {
omode = 0;
imode = 1;
} else {
omode = 1;
imode = 0;
}
int steep = abs(y1 - y0) > abs(x1 - x0);
if (steep) {
SWAP(x0, y0);
SWAP(x1, y1);
}
if (x0 > x1) {
SWAP(x0, x1);
SWAP(y0, y1);
}
int deltax = x1 - x0;
unsigned int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
unsigned int y = y0;
unsigned int x;
if (y0 < y1) {
ystep = 1;
} else {
ystep = -1;
}
// Draw the outline.
for (x = x0; x < x1; x++) {
if (steep) {
write_pixel_lm(y - 1, x, mmode, omode);
write_pixel_lm(y + 1, x, mmode, omode);
write_pixel_lm(y, x - 1, mmode, omode);
write_pixel_lm(y, x + 1, mmode, omode);
} else {
write_pixel_lm(x - 1, y, mmode, omode);
write_pixel_lm(x + 1, y, mmode, omode);
write_pixel_lm(x, y - 1, mmode, omode);
write_pixel_lm(x, y + 1, mmode, omode);
}
error -= deltay;
if (error < 0) {
y += ystep;
error += deltax;
}
}
// Now draw the innards.
error = deltax / 2;
y = y0;
for (x = x0; x < x1; x++) {
if (steep) {
write_pixel_lm(y, x, mmode, imode);
} else {
write_pixel_lm(x, y, mmode, imode);
}
error -= deltay;
if (error < 0) {
y += ystep;
error += deltax;
}
}
}
/**
@ -994,12 +945,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
*/
void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode)
{
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
if(xoff > 0)
WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
int16_t firstmask = word >> xoff;
int16_t lastmask = word << (16 - xoff);
WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
if (xoff > 0) {
WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
}
}
/**
@ -1019,12 +971,13 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
*/
void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
if(xoff > 0)
WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
if (xoff > 0) {
WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
}
}
/**
@ -1044,12 +997,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
*/
void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
if(xoff > 0)
WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
if (xoff > 0) {
WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
}
}
@ -1067,8 +1021,8 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
*/
void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode)
{
write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
}
/**
@ -1079,22 +1033,22 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr,
*/
int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
{
// First locate the font struct.
if(font > SIZEOF_ARRAY(fonts))
return 0; // font does not exist, exit.*/
// Load the font info; IDs are always sequential.
*font_info = fonts[font];
// Locate character in font lookup table. (If required.)
if(lookup != NULL)
{
*lookup = font_info->lookup[ch];
if(*lookup == 0xff)
return 0; // character doesn't exist, don't bother writing it.
}
return 1;
// First locate the font struct.
if ((unsigned int)font > SIZEOF_ARRAY(fonts)) {
return 0; // font does not exist, exit.
}
// Load the font info; IDs are always sequential.
*font_info = fonts[font];
// Locate character in font lookup table. (If required.)
if (lookup != NULL) {
*lookup = font_info->lookup[ch];
if (*lookup == 0xff) {
return 0; // character doesn't exist, don't bother writing it.
}
}
return 1;
}
/**
* write_char16: Draw a character on the current draw buffer.
* Currently supports outlined characters and characters with
@ -1108,12 +1062,12 @@ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *loo
*/
void write_char16(char ch, unsigned int x, unsigned int y, int font)
{
int yy, addr_temp, row, row_temp, xshift;
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
struct FontEntry font_info;
//char lookup = 0;
fetch_font_info(0, font, &font_info, NULL);
// Compute starting address (for x,y) of character.
int addr = CALC_BUFF_ADDR(x, y);
int wbit = CALC_BIT_IN_WORD(x);
@ -1122,58 +1076,55 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
// How big is the character? We handle characters up to 8 pixels
// wide for now. Support for large characters may be added in future.
{
// Ensure we don't overflow.
if(x + wbit > GRAPHICS_WIDTH_REAL)
return;
// Load data pointer.
row = ch * font_info.height;
row_temp = row;
addr_temp = addr;
xshift = 16 - font_info.width;
// We can write mask words easily.
for(yy = y; yy < y + font_info.height; yy++)
{
if(font==3)
write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
else
write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
// Level bits are more complicated. We need to set or clear
// level bits, but only where the mask bit is set; otherwise,
// we need to leave them alone. To do this, for each word, we
// construct an AND mask and an OR mask, and apply each individually.
row = row_temp;
addr = addr_temp;
for(yy = y; yy < y + font_info.height; yy++)
{
if(font==3)
{
level_bits = font_frame12x18[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
or_mask = font_mask12x18[row] << xshift;
and_mask = (font_mask12x18[row] & level_bits) << xshift;
} else {
level_bits = font_frame8x10[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
or_mask = font_mask8x10[row] << xshift;
and_mask = (font_mask8x10[row] & level_bits) << xshift;
}
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD))
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
// Ensure we don't overflow.
if (x + wbit > GRAPHICS_WIDTH_REAL) {
return;
}
// Load data pointer.
row = ch * font_info.height;
row_temp = row;
addr_temp = addr;
xshift = 16 - font_info.width;
// We can write mask words easily.
for (yy = y; yy < y + font_info.height; yy++) {
if (font == 3) {
write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
} else {
write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
}
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
// Level bits are more complicated. We need to set or clear
// level bits, but only where the mask bit is set; otherwise,
// we need to leave them alone. To do this, for each word, we
// construct an AND mask and an OR mask, and apply each individually.
row = row_temp;
addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) {
if (font == 3) {
level_bits = font_frame12x18[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
or_mask = font_mask12x18[row] << xshift;
and_mask = (font_mask12x18[row] & level_bits) << xshift;
} else {
level_bits = font_frame8x10[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
or_mask = font_mask8x10[row] << xshift;
and_mask = (font_mask8x10[row] & level_bits) << xshift;
}
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD))
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
}
}
/**
* write_char: Draw a character on the current draw buffer.
* Currently supports outlined characters and characters with
@ -1187,109 +1138,109 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
*/
void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
{
int yy, addr_temp, row, row_temp, xshift;
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
struct FontEntry font_info;
char lookup = 0;
fetch_font_info(ch, font, &font_info, &lookup);
// Compute starting address (for x,y) of character.
int addr = CALC_BUFF_ADDR(x, y);
int wbit = CALC_BIT_IN_WORD(x);
unsigned int addr = CALC_BUFF_ADDR(x, y);
unsigned int wbit = CALC_BIT_IN_WORD(x);
// If font only supports lowercase or uppercase, make the letter
// lowercase or uppercase.
/*if(font_info.flags & FONT_LOWERCASE_ONLY)
ch = tolower(ch);
if(font_info.flags & FONT_UPPERCASE_ONLY)
ch = toupper(ch);*/
ch = tolower(ch);
if(font_info.flags & FONT_UPPERCASE_ONLY)
ch = toupper(ch);*/
fetch_font_info(ch, font, &font_info, &lookup);
// How big is the character? We handle characters up to 8 pixels
// wide for now. Support for large characters may be added in future.
if(font_info.width <= 8)
{
// Ensure we don't overflow.
if(x + wbit > GRAPHICS_WIDTH_REAL)
return;
// Load data pointer.
row = lookup * font_info.height * 2;
row_temp = row;
addr_temp = addr;
xshift = 16 - font_info.width;
// We can write mask words easily.
for(yy = y; yy < y + font_info.height; yy++)
{
write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
// Level bits are more complicated. We need to set or clear
// level bits, but only where the mask bit is set; otherwise,
// we need to leave them alone. To do this, for each word, we
// construct an AND mask and an OR mask, and apply each individually.
row = row_temp;
addr = addr_temp;
for(yy = y; yy < y + font_info.height; yy++)
{
level_bits = font_info.data[row + font_info.height];
if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
or_mask = font_info.data[row] << xshift;
and_mask = (font_info.data[row] & level_bits) << xshift;
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD))
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
if (font_info.width <= 8) {
// Ensure we don't overflow.
if (x + wbit > GRAPHICS_WIDTH_REAL) {
return;
}
// Load data pointer.
row = lookup * font_info.height * 2;
row_temp = row;
addr_temp = addr;
xshift = 16 - font_info.width;
// We can write mask words easily.
for (yy = y; yy < y + font_info.height; yy++) {
write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
// Level bits are more complicated. We need to set or clear
// level bits, but only where the mask bit is set; otherwise,
// we need to leave them alone. To do this, for each word, we
// construct an AND mask and an OR mask, and apply each individually.
row = row_temp;
addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) {
level_bits = font_info.data[row + font_info.height];
if (!(flags & FONT_INVERT)) {
// data is normally inverted
level_bits = ~level_bits;
}
or_mask = font_info.data[row] << xshift;
and_mask = (font_info.data[row] & level_bits) << xshift;
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD))
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8;
row++;
}
}
}
/**
* calc_text_dimensions: Calculate the dimensions of a
* string in a given font. Supports new lines and
* carriage returns in text.
*
* @param str string to calculate dimensions of
* @param font_info font info structure
* @param xs horizontal spacing
* @param ys vertical spacing
* @param dim return result: struct FontDimensions
*/
* calc_text_dimensions: Calculate the dimensions of a
* string in a given font. Supports new lines and
* carriage returns in text.
*
* @param str string to calculate dimensions of
* @param font_info font info structure
* @param xs horizontal spacing
* @param ys vertical spacing
* @param dim return result: struct FontDimensions
*/
void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim)
{
int max_length = 0, line_length = 0, lines = 1;
while(*str != 0)
{
line_length++;
if(*str == '\n' || *str == '\r')
{
if(line_length > max_length)
max_length = line_length;
line_length = 0;
lines++;
}
str++;
while (*str != 0) {
line_length++;
if (*str == '\n' || *str == '\r') {
if (line_length > max_length) {
max_length = line_length;
}
line_length = 0;
lines++;
}
str++;
}
if (line_length > max_length) {
max_length = line_length;
}
if(line_length > max_length)
max_length = line_length;
dim->width = max_length * (font.width + xs);
dim->height = lines * (font.height + ys);
}
/**
* write_string: Draw a string on the screen with certain
* alignment parameters.
*
* @param str string to write
* @param x x coordinate
* @param y y coordinate
* @param xs horizontal spacing
* @param ys horizontal spacing
* @param va vertical align
* @param ha horizontal align
* @param flags flags (passed to write_char)
* @param font font
*/
* write_string: Draw a string on the screen with certain
* alignment parameters.
*
* @param str string to write
* @param x x coordinate
* @param y y coordinate
* @param xs horizontal spacing
* @param ys horizontal spacing
* @param va vertical align
* @param ha horizontal align
* @param flags flags (passed to write_char)
* @param font font
*/
void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font)
{
int xx = 0, yy = 0, xx_original = 0;
@ -1298,56 +1249,63 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
// Determine font info and dimensions/position of the string.
fetch_font_info(0, font, &font_info, NULL);
calc_text_dimensions(str, font_info, xs, ys, &dim);
switch(va)
{
case TEXT_VA_TOP: yy = y; break;
case TEXT_VA_MIDDLE: yy = y - (dim.height / 2); break;
case TEXT_VA_BOTTOM: yy = y - dim.height; break;
switch (va) {
case TEXT_VA_TOP:
yy = y;
break;
case TEXT_VA_MIDDLE:
yy = y - (dim.height / 2);
break;
case TEXT_VA_BOTTOM:
yy = y - dim.height;
break;
}
switch(ha)
{
case TEXT_HA_LEFT: xx = x; break;
case TEXT_HA_CENTER: xx = x - (dim.width / 2); break;
case TEXT_HA_RIGHT: xx = x - dim.width; break;
switch (ha) {
case TEXT_HA_LEFT:
xx = x;
break;
case TEXT_HA_CENTER:
xx = x - (dim.width / 2);
break;
case TEXT_HA_RIGHT:
xx = x - dim.width;
break;
}
// Then write each character.
xx_original = xx;
while(*str != 0)
{
if(*str == '\n' || *str == '\r')
{
yy += ys + font_info.height;
xx = xx_original;
}
else
{
if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL)
{
if(font_info.id<2)
write_char(*str, xx, yy, flags, font);
else
write_char16(*str, xx, yy, font);
}
xx += font_info.width + xs;
}
str++;
while (*str != 0) {
if (*str == '\n' || *str == '\r') {
yy += ys + font_info.height;
xx = xx_original;
} else {
if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
if (font_info.id < 2) {
write_char(*str, xx, yy, flags, font);
} else {
write_char16(*str, xx, yy, font);
}
}
xx += font_info.width + xs;
}
str++;
}
}
/**
* write_string_formatted: Draw a string with format escape
* sequences in it. Allows for complex text effects.
*
* @param str string to write (with format data)
* @param x x coordinate
* @param y y coordinate
* @param xs default horizontal spacing
* @param ys default horizontal spacing
* @param va vertical align
* @param ha horizontal align
* @param flags flags (passed to write_char)
*/
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags)
* write_string_formatted: Draw a string with format escape
* sequences in it. Allows for complex text effects.
*
* @param str string to write (with format data)
* @param x x coordinate
* @param y y coordinate
* @param xs default horizontal spacing
* @param ys default horizontal spacing
* @param va vertical align
* @param ha horizontal align
* @param flags flags (passed to write_char)
*/
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys,
__attribute__((unused)) int va, __attribute__((unused)) int ha, int flags)
{
int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
struct FontEntry font_info;
@ -1363,78 +1321,76 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
// work out a bounding box. We'll parse again for the final output.
// This is a simple state machine parser.
char *ostr = str;
while(*str)
{
if(*str == '<' && fcode == 1) // escape code: skip
fcode = 0;
if(*str == '<' && fcode == 0) // begin format code?
{
fcode = 1;
fptr = 0;
while (*str) {
if (*str == '<' && fcode == 1) {
// escape code: skip
fcode = 0;
}
if (*str == '<' && fcode == 0) {
// begin format code?
fcode = 1;
fptr = 0;
}
if (*str == '>' && fcode == 1) {
fcode = 0;
if (strcmp(fstack, "B")) {
// switch to "big" font (font #1)
fwidth = bigfontwidth;
fheight = bigfontheight;
} else if (strcmp(fstack, "S")) {
// switch to "small" font (font #0)
fwidth = smallfontwidth;
fheight = smallfontheight;
}
if(*str == '>' && fcode == 1)
{
fcode = 0;
if(strcmp(fstack, "B")) // switch to "big" font (font #1)
{
fwidth = bigfontwidth;
fheight = bigfontheight;
}
else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
{
fwidth = smallfontwidth;
fheight = smallfontheight;
}
if(fheight > max_height)
max_height = fheight;
// Skip over this byte. Go to next byte.
str++;
continue;
}
if(*str != '<' && *str != '>' && fcode == 1)
{
// Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes
{
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if(fcode == 0)
{
// Not a format code, raw text.
xx += fwidth + xs;
if(*str == '\n')
{
if(xx > max_xx)
max_xx = xx;
xx = x;
yy += fheight + ys;
}
if (fheight > max_height) {
max_height = fheight;
}
// Skip over this byte. Go to next byte.
str++;
continue;
}
if (*str != '<' && *str != '>' && fcode == 1) {
// Add to the format stack (up to 10 bytes.)
if (fptr > 10) {
// stop adding bytes
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if (fcode == 0) {
// Not a format code, raw text.
xx += fwidth + xs;
if (*str == '\n') {
if (xx > max_xx) {
max_xx = xx;
}
xx = x;
yy += fheight + ys;
}
}
str++;
}
// Reset string pointer.
str = ostr;
// Now we've parsed it and got a bbox, we need to work out the dimensions of it
// and how to align it.
/*int width = max_xx - x;
int height = yy - y;
int ay, ax;
switch(va)
{
case TEXT_VA_TOP: ay = yy; break;
case TEXT_VA_MIDDLE: ay = yy - (height / 2); break;
case TEXT_VA_BOTTOM: ay = yy - height; break;
}
switch(ha)
{
case TEXT_HA_LEFT: ax = x; break;
case TEXT_HA_CENTER: ax = x - (width / 2); break;
case TEXT_HA_RIGHT: ax = x - width; break;
}*/
int height = yy - y;
int ay, ax;
switch(va)
{
case TEXT_VA_TOP: ay = yy; break;
case TEXT_VA_MIDDLE: ay = yy - (height / 2); break;
case TEXT_VA_BOTTOM: ay = yy - height; break;
}
switch(ha)
{
case TEXT_HA_LEFT: ax = x; break;
case TEXT_HA_CENTER: ax = x - (width / 2); break;
case TEXT_HA_RIGHT: ax = x - width; break;
}*/
// So ax,ay is our new text origin. Parse the text format again and paint
// the text on the display.
fcode = 0;
@ -1442,204 +1398,196 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
font = 0;
xx = 0;
yy = 0;
while(*str)
{
if(*str == '<' && fcode == 1) // escape code: skip
fcode = 0;
if(*str == '<' && fcode == 0) // begin format code?
{
fcode = 1;
fptr = 0;
}
if(*str == '>' && fcode == 1)
{
fcode = 0;
if(strcmp(fstack, "B")) // switch to "big" font (font #1)
{
fwidth = bigfontwidth;
fheight = bigfontheight;
font = 1;
}
else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
{
fwidth = smallfontwidth;
fheight = smallfontheight;
font = 0;
}
// Skip over this byte. Go to next byte.
str++;
continue;
}
if(*str != '<' && *str != '>' && fcode == 1)
{
// Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes
{
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if(fcode == 0)
{
// Not a format code, raw text. So we draw it.
// TODO - different font sizes.
write_char(*str, xx, yy + (max_height - fheight), flags, font);
xx += fwidth + xs;
if(*str == '\n')
{
if(xx > max_xx)
max_xx = xx;
xx = x;
yy += fheight + ys;
}
while (*str) {
if (*str == '<' && fcode == 1) {
// escape code: skip
fcode = 0;
}
if (*str == '<' && fcode == 0) {
// begin format code?
fcode = 1;
fptr = 0;
}
if (*str == '>' && fcode == 1) {
fcode = 0;
if (strcmp(fstack, "B")) {
// switch to "big" font (font #1)
fwidth = bigfontwidth;
fheight = bigfontheight;
font = 1;
} else if (strcmp(fstack, "S")) {
// switch to "small" font (font #0)
fwidth = smallfontwidth;
fheight = smallfontheight;
font = 0;
}
// Skip over this byte. Go to next byte.
str++;
continue;
}
if (*str != '<' && *str != '>' && fcode == 1) {
// Add to the format stack (up to 10 bytes.)
if (fptr > 10) {
// stop adding bytes
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if (fcode == 0) {
// Not a format code, raw text. So we draw it.
// TODO - different font sizes.
write_char(*str, xx, yy + (max_height - fheight), flags, font);
xx += fwidth + xs;
if (*str == '\n') {
if (xx > max_xx) {
max_xx = xx;
}
xx = x;
yy += fheight + ys;
}
}
str++;
}
}
//SUPEROSD-
// graphics
void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size)
{
int16_t a = mySin(roll+360);
int16_t b = myCos(roll+360);
int16_t c = mySin(roll+90+360)*5/100;
int16_t d = myCos(roll+90+360)*5/100;
int16_t a = mySin(roll + 360);
int16_t b = myCos(roll + 360);
int16_t c = mySin(roll + 90 + 360) * 5 / 100;
int16_t d = myCos(roll + 90 + 360) * 5 / 100;
int16_t k;
int16_t l;
int16_t k;
int16_t l;
int16_t indi30x1=myCos(30)*(size/2+1) / 100;
int16_t indi30y1=mySin(30)*(size/2+1) / 100;
int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100;
int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100;
int16_t indi30x2=myCos(30)*(size/2+4) / 100;
int16_t indi30y2=mySin(30)*(size/2+4) / 100;
int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100;
int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100;
int16_t indi60x1=myCos(60)*(size/2+1) / 100;
int16_t indi60y1=mySin(60)*(size/2+1) / 100;
int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100;
int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100;
int16_t indi60x2=myCos(60)*(size/2+4) / 100;
int16_t indi60y2=mySin(60)*(size/2+4) / 100;
int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100;
int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100;
pitch=pitch%90;
if(pitch>90)
{
pitch=pitch-90;
}
if(pitch<-90)
{
pitch=pitch+90;
}
a = (a * (size/2)) / 100;
b = (b * (size/2)) / 100;
pitch = pitch % 90;
if (pitch > 90) {
pitch = pitch - 90;
}
if (pitch < -90) {
pitch = pitch + 90;
}
a = (a * (size / 2)) / 100;
b = (b * (size / 2)) / 100;
if(roll<-90 || roll>90)
pitch=pitch*-1;
k = a*pitch/90;
l = b*pitch/90;
if (roll < -90 || roll > 90) {
pitch = pitch * -1;
}
k = a * pitch / 90;
l = b * pitch / 90;
// scale
//0
//drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
//drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
write_line_outlined((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1,0,0,0,1);
write_line_outlined((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1,0,0,0,1);
// scale
//0
//drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
//drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
//30
//drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
//drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
write_line_outlined((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2,0,0,0,1);
write_line_outlined((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2,0,0,0,1);
//60
//drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
//drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
write_line_outlined((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2,0,0,0,1);
write_line_outlined((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2,0,0,0,1);
//90
//drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
write_line_outlined((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1),0,0,0,1);
//30
//drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
//drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
//60
//drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
//drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
//90
//drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1);
//roll
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line
//"wingtips"
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1);
write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);
//roll
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a,0,0,0,1); //Direction line
//"wingtips"
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c,0,0,0,1);
write_line_outlined((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a,0,0,0,1);
//pitch
//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1);
//pitch
//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,0,0,0,1);
//drawCircle(x-1, y-1, 5);
//write_circle_outlined(x-1, y-1, 5,0,0,0,1);
//drawCircle(x-1, y-1, size/2+4);
//write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
//drawCircle(x-1, y-1, 5);
//write_circle_outlined(x-1, y-1, 5,0,0,0,1);
//drawCircle(x-1, y-1, size/2+4);
//write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
}
void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
{
int i=0;
int batteryLines;
//top
/*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);
int i = 0;
int batteryLines;
//top
/*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);
drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
//bottom
drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
//left
drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);
drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
//bottom
drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
//left
drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);
//right
drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
//right
drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
write_rectangle_outlined((x)-1, (y)-1+2,size,size*3,0,1);
write_vline_lm((x)-1+(size/2+size/4)+1,(y)-2,(y)-1+1,0,1);
write_vline_lm((x)-1+(size/2-size/4)-1,(y)-2,(y)-1+1,0,1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-2,0,1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1,1,1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1+1,1,1);
write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1);
write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1);
write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1);
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1);
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1);
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1);
batteryLines = battery*(size*3-2)/100;
for(i=0;i<batteryLines;i++)
{
write_hline_lm((x)-1,(x)-1 + size,(y)-1+size*3-i,1,1);
}
batteryLines = battery * (size * 3 - 2) / 100;
for (i = 0; i < batteryLines; i++) {
write_hline_lm((x) - 1, (x) - 1 + size, (y) - 1 + size * 3 - i, 1, 1);
}
}
void printTime(uint16_t x, uint16_t y) {
char temp[9]={0};
sprintf(temp,"%02d:%02d:%02d",timex.hour,timex.min,timex.sec);
//printTextFB(x,y,temp);
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
void printTime(uint16_t x, uint16_t y)
{
char temp[9] =
{ 0 };
sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
//printTextFB(x,y,temp);
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
}
/*
void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
char temp[9]={0};
char updown=' ';
uint16_t charx=x/16;
if(dir==0)
updown=24;
if(dir==1)
updown=25;
sprintf(temp,"%c%6dm",updown,alt);
printTextFB(charx,y+2,temp);
// frame
drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
}*/
char temp[9]={0};
char updown=' ';
uint16_t charx=x/16;
if(dir==0)
updown=24;
if(dir==1)
updown=25;
sprintf(temp,"%c%6dm",updown,alt);
printTextFB(charx,y+2,temp);
// frame
drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
}*/
/**
* hud_draw_vertical_scale: Draw a vertical scale.
@ -1658,148 +1606,134 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
* @param max_val maximum expected value (used to compute size of arrow ticker)
* @param flags special flags (see hud.h.)
*/
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int boundtick_len, int max_val, int flags)
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
int boundtick_len, __attribute__((unused)) int max_val, int flags)
{
char temp[15];//, temp2[15];
struct FontEntry font_info;
struct FontDimensions dim;
// Halign should be in a small span.
//MY_ASSERT(halign >= -1 && halign <= 1);
// Compute the position of the elements.
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
if(halign == -1)
{
majtick_start = x;
majtick_end = x + majtick_len;
mintick_start = x;
mintick_end = x + mintick_len;
boundtick_start = x;
boundtick_end = x + boundtick_len;
}
else if(halign == +1)
{
x=x-GRAPHICS_HDEADBAND;
majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
}
// Retrieve width of large font (font #0); from this calculate the x spacing.
fetch_font_info(0, 0, &font_info, NULL);
int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly??
int text_x_spacing = arrow_len;
int max_text_y = 0, text_length = 0;
int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
// For -(range / 2) to +(range / 2), draw the scale.
int range_2 = range / 2; //, height_2 = height / 2;
int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
// Iterate through each step.
for(r = -range_2; r <= +range_2; r++)
{
style = 0;
rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
rv = -rr + range_2; // for number display
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
rr += majtick_step / 2;
if(rr % majtick_step == 0)
style = 1; // major tick
else if(rr % mintick_step == 0)
style = 2; // minor tick
char temp[15]; //, temp2[15];
struct FontEntry font_info;
struct FontDimensions dim;
// Halign should be in a small span.
//MY_ASSERT(halign >= -1 && halign <= 1);
// Compute the position of the elements.
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
if (halign == -1) {
majtick_start = x;
majtick_end = x + majtick_len;
mintick_start = x;
mintick_end = x + mintick_len;
boundtick_start = x;
boundtick_end = x + boundtick_len;
} else if (halign == +1) {
x = x - GRAPHICS_HDEADBAND;
majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
}
// Retrieve width of large font (font #0); from this calculate the x spacing.
fetch_font_info(0, 0, &font_info, NULL);
int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly??
int text_x_spacing = arrow_len;
int max_text_y = 0, text_length = 0;
int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
// For -(range / 2) to +(range / 2), draw the scale.
int range_2 = range / 2; //, height_2 = height / 2;
int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
// Iterate through each step.
for (r = -range_2; r <= +range_2; r++) {
style = 0;
rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
rv = -rr + range_2; // for number display
if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
rr += majtick_step / 2;
if (rr % majtick_step == 0)
style = 1; // major tick
else if (rr % mintick_step == 0)
style = 2; // minor tick
else
style = 0;
if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
continue;
if (style) {
// Calculate y position.
ys = ((long int) (r * height) / (long int) range) + y;
//sprintf(temp, "ys=%d", ys);
//con_puts(temp, 0);
// Depending on style, draw a minor or a major tick.
if (style == 1) {
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
memset(temp, ' ', 10);
//my_itoa(rv, temp);
sprintf(temp, "%d", rv);
text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
if (text_length > max_text_y)
max_text_y = text_length;
if (halign == -1)
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
else
style = 0;
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
continue;
if(style)
{
// Calculate y position.
ys = ((long int)(r * height) / (long int)range) + y;
//sprintf(temp, "ys=%d", ys);
//con_puts(temp, 0);
// Depending on style, draw a minor or a major tick.
if(style == 1)
{
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
memset(temp, ' ', 10);
//my_itoa(rv, temp);
sprintf(temp,"%d",rv);
text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
if(text_length > max_text_y)
max_text_y = text_length;
if(halign == -1)
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
else
write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
}
else if(style == 2)
write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
}
write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
} else if (style == 2)
write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
}
// Generate the string for the value, as well as calculating its dimensions.
memset(temp, ' ', 10);
//my_itoa(v, temp);
sprintf(temp,"%d",v);
// TODO: add auto-sizing.
calc_text_dimensions(temp, font_info, 1, 0, &dim);
int xx = 0, i = 0;
if(halign == -1)
xx = majtick_end + text_x_spacing;
else
xx = majtick_end - text_x_spacing;
// Draw an arrow from the number to the point.
for(i = 0; i < arrow_len; i++)
{
if(halign == -1)
{
write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
}
else
{
write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
}
// FIXME
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
}
// Generate the string for the value, as well as calculating its dimensions.
memset(temp, ' ', 10);
//my_itoa(v, temp);
sprintf(temp, "%d", v);
// TODO: add auto-sizing.
calc_text_dimensions(temp, font_info, 1, 0, &dim);
int xx = 0, i = 0;
if (halign == -1)
xx = majtick_end + text_x_spacing;
else
xx = majtick_end - text_x_spacing;
// Draw an arrow from the number to the point.
for (i = 0; i < arrow_len; i++) {
if (halign == -1) {
write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
} else {
write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
}
if(halign == -1)
{
write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
}
else
{
write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
}
// Draw the text.
if(halign == -1)
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
else
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
// Then, add a slow cut off on the edges, so the text doesn't sharply
// disappear. We simply clear the areas above and below the ticker, and we
// use little markers on the edges.
if(halign == -1)
{
write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
}
else
{
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
}
write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
// FIXME
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
}
if (halign == -1) {
write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
} else {
write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
}
// Draw the text.
if (halign == -1)
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
else
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
// Then, add a slow cut off on the edges, so the text doesn't sharply
// disappear. We simply clear the areas above and below the ticker, and we
// use little markers on the edges.
if (halign == -1) {
write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
0);
write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
0);
} else {
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
}
write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
}
/**
@ -1816,601 +1750,613 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
* @param majtick_len major tick length
* @param flags special flags (see hud.h.)
*/
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags)
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, __attribute__((unused)) int flags)
{
v %= 360; // wrap, just in case.
struct FontEntry font_info;
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
char headingstr[4];
majtick_start = y;
majtick_end = y - majtick_len;
mintick_start = y;
mintick_end = y - mintick_len;
textoffset = 8;
int r, style, rr, xs; // rv,
int range_2 = range / 2;
for(r = -range_2; r <= +range_2; r++)
{
style = 0;
rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
//rv = -rr + range_2; // for number display
if(rr % majtick_step == 0)
style = 1; // major tick
else if(rr % mintick_step == 0)
style = 2; // minor tick
if(style)
{
// Calculate x position.
xs = ((long int)(r * width) / (long int)range) + x;
// Draw it.
if(style == 1)
{
write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
// Draw heading above this tick.
// If it's not one of north, south, east, west, draw the heading.
// Otherwise, draw one of the identifiers.
if(rr % 90 != 0)
{
// We abbreviate heading to two digits. This has the side effect of being easy to compute.
headingstr[0] = '0' + (rr / 100);
headingstr[1] = '0' + ((rr / 10) % 10);
headingstr[2] = 0;
headingstr[3] = 0; // nul to terminate
}
else
{
switch(rr)
{
case 0: headingstr[0] = 'N'; break;
case 90: headingstr[0] = 'E'; break;
case 180: headingstr[0] = 'S'; break;
case 270: headingstr[0] = 'W'; break;
}
headingstr[1] = 0;
headingstr[2] = 0;
headingstr[3] = 0;
}
// +1 fudge...!
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
}
else if(style == 2)
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
v %= 360; // wrap, just in case.
struct FontEntry font_info;
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
char headingstr[4];
majtick_start = y;
majtick_end = y - majtick_len;
mintick_start = y;
mintick_end = y - mintick_len;
textoffset = 8;
int r, style, rr, xs; // rv,
int range_2 = range / 2;
for (r = -range_2; r <= +range_2; r++) {
style = 0;
rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
//rv = -rr + range_2; // for number display
if (rr % majtick_step == 0)
style = 1; // major tick
else if (rr % mintick_step == 0)
style = 2; // minor tick
if (style) {
// Calculate x position.
xs = ((long int) (r * width) / (long int) range) + x;
// Draw it.
if (style == 1) {
write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
// Draw heading above this tick.
// If it's not one of north, south, east, west, draw the heading.
// Otherwise, draw one of the identifiers.
if (rr % 90 != 0) {
// We abbreviate heading to two digits. This has the side effect of being easy to compute.
headingstr[0] = '0' + (rr / 100);
headingstr[1] = '0' + ((rr / 10) % 10);
headingstr[2] = 0;
headingstr[3] = 0; // nul to terminate
} else {
switch (rr) {
case 0:
headingstr[0] = 'N';
break;
case 90:
headingstr[0] = 'E';
break;
case 180:
headingstr[0] = 'S';
break;
case 270:
headingstr[0] = 'W';
break;
}
headingstr[1] = 0;
headingstr[2] = 0;
headingstr[3] = 0;
}
// +1 fudge...!
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
} else if (style == 2)
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
}
// Then, draw a rectangle with the present heading in it.
// We want to cover up any other markers on the bottom.
// First compute font size.
fetch_font_info(0, 3, &font_info, NULL);
int text_width = (font_info.width + 1) * 3;
int rect_width = text_width + 2;
write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
headingstr[0] = '0' + (v / 100);
headingstr[1] = '0' + ((v / 10) % 10);
headingstr[2] = '0' + (v % 10);
headingstr[3] = 0;
write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
}
// Then, draw a rectangle with the present heading in it.
// We want to cover up any other markers on the bottom.
// First compute font size.
fetch_font_info(0, 3, &font_info, NULL);
int text_width = (font_info.width + 1) * 3;
int rect_width = text_width + 2;
write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
headingstr[0] = '0' + (v / 100);
headingstr[1] = '0' + ((v / 10) % 10);
headingstr[2] = '0' + (v % 10);
headingstr[3] = 0;
write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
}
// CORE draw routines end here
void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size )
void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size)
{
float alpha;
uint8_t vertical=0,horizontal=0;
int16_t x1,x2;
int16_t y1,y2;
int16_t refx,refy;
alpha=DEG2RAD(angle);
refx=l_x + size/2;
refy=l_y + size/2;
float alpha;
uint8_t vertical = 0, horizontal = 0;
int16_t x1, x2;
int16_t y1, y2;
int16_t refx, refy;
alpha = DEG2RAD(angle);
refx = l_x + size / 2;
refy = l_y + size / 2;
//
float k=0;
float dx = sinf(alpha)*(pitch/90.0f*(size/2));
float dy = cosf(alpha)*(pitch/90.0f*(size/2));
int16_t x0 = (size/2)-dx;
int16_t y0 = (size/2)+dy;
// calculate the line function
if((angle < 90.0f) && (angle > -90.0f))
{
//
float k = 0;
float dx = sinf(alpha) * (pitch / 90.0f * (size / 2));
float dy = cosf(alpha) * (pitch / 90.0f * (size / 2));
int16_t x0 = (size / 2) - dx;
int16_t y0 = (size / 2) + dy;
// calculate the line function
if ((angle < 90.0f) && (angle > -90)) {
vertical = 0;
if(fabsf(angle) < 1e-5f) {
horizontal = 1;
} else {
k = tanf(alpha);
}
}
else
{
vertical = 1;
}
} else {
vertical = 1;
}
// crossing point of line
if(!vertical && !horizontal)
{
// y-y0=k(x-x0)
int16_t x=0;
int16_t y=k*(x-x0)+y0;
// find right crossing point
x1=x;
y1=y;
if(y<0)
{
y1=0;
x1=((y1-y0)+k*x0)/k;
}
if(y>size)
{
y1=size;
x1=((y1-y0)+k*x0)/k;
}
// left crossing point
x=size;
y=k*(x-x0)+y0;
x2=x;
y2=y;
if(y<0)
{
y2=0;
x2=((y2-y0)+k*x0)/k;
}
if(y>size)
{
y2=size;
x2=((y2-y0)+k*x0)/k;
}
// move to location
// horizon line
write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
//fill
if(angle <= 0.0f && angle > -90.0f)
{
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y2;i<size;i++)
{
x2=((i-y0)+k*x0)/k;
if(x2>size)
x2=size;
if(x2<0)
x2=0;
write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1);
}
}
else if(angle < -90.0f)
{
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y2;i++)
{
x2=((i-y0)+k*x0)/k;
if(x2>size)
x2=size;
if(x2<0)
x2=0;
write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1);
}
}
else if(angle > 0.0f && angle < 90.0f)
{
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y1;i<size;i++)
{
x2=((i-y0)+k*x0)/k;
if(x2>size)
x2=size;
if(x2<0)
x2=0;
write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1);
}
}
else if(angle > 90.0f)
{
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y1;i++)
{
x2=((i-y0)+k*x0)/k;
if(x2>size)
x2=size;
if(x2<0)
x2=0;
write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1);
}
}
}
else if(vertical)
{
// horizon line
write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1);
if(angle >= 90.0f)
{
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<size;i++)
{
write_hline_lm(0+l_x,x0+l_x,i+l_y,1,1);
}
}
else
{
//write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<size;i++)
{
write_hline_lm(size+l_x,x0+l_x,i+l_y,1,1);
}
}
}
else if(horizontal)
{
// horizon line
write_hline_outlined(0+l_x,size+l_x,y0+l_y,0,0,0,1);
if(angle<0)
{
//write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y0;i++)
{
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
}
}
else
{
//write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y0;i<size;i++)
{
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
}
}
}
// crossing point of line
if (!vertical && !horizontal) {
// y-y0=k(x-x0)
int16_t x = 0;
int16_t y = k * (x - x0) + y0;
// find right crossing point
x1 = x;
y1 = y;
if (y < 0) {
y1 = 0;
x1 = ((y1 - y0) + k * x0) / k;
}
if (y > size) {
y1 = size;
x1 = ((y1 - y0) + k * x0) / k;
}
// left crossing point
x = size;
y = k * (x - x0) + y0;
x2 = x;
y2 = y;
if (y < 0) {
y2 = 0;
x2 = ((y2 - y0) + k * x0) / k;
}
if (y > size) {
y2 = size;
x2 = ((y2 - y0) + k * x0) / k;
}
// move to location
// horizon line
write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
//fill
if (angle <= 0.0f && angle > -90.0f) {
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = y2; i < size; i++) {
x2 = ((i - y0) + k * x0) / k;
if (x2 > size) {
x2 = size;
}
if (x2 < 0) {
x2 = 0;
}
write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
}
} else if (angle < -90.0f) {
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = 0; i < y2; i++) {
x2 = ((i - y0) + k * x0) / k;
if (x2 > size) {
x2 = size;
}
if (x2 < 0) {
x2 = 0;
}
write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
}
} else if (angle > 0.0f && angle < 90.0f) {
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = y1; i < size; i++) {
x2 = ((i - y0) + k * x0) / k;
if (x2 > size) {
x2 = size;
}
if (x2 < 0) {
x2 = 0;
}
write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
}
} else if (angle > 90.0f) {
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = 0; i < y1; i++) {
x2 = ((i - y0) + k * x0) / k;
if (x2 > size) {
x2 = size;
}
if (x2 < 0) {
x2 = 0;
}
write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
}
}
} else if (vertical) {
// horizon line
write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
if (angle >= 90.0f) {
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = 0; i < size; i++) {
write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
}
} else {
//write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = 0; i < size; i++) {
write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
}
}
} else if (horizontal) {
// horizon line
write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
if (angle < 0) {
//write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = 0; i < y0; i++) {
write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
}
} else {
//write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for (int i = y0; i < size; i++) {
write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
}
}
}
//sides
write_line_outlined(l_x,l_y,l_x,l_y+size,0,0,0,1);
write_line_outlined(l_x+size,l_y,l_x+size,l_y+size,0,0,0,1);
//plane
write_line_outlined(refx-5,refy,refx+6,refy,0,0,0,1);
write_line_outlined(refx,refy,refx,refy-3,0,0,0,1);
//sides
write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1);
write_line_outlined(l_x + size, l_y, l_x + size, l_y + size, 0, 0, 0, 1);
//plane
write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1);
write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1);
}
void introText(){
write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
void introText()
{
write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
}
void introGraphics() {
/* logo */
int image=0;
struct splashEntry splash_info;
splash_info = splash[image];
void introGraphics()
{
/* logo */
int image = 0;
struct splashEntry splash_info;
splash_info = splash[image];
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image);
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);
/* frame */
drawBox(APPLY_HDEADBAND(0),APPLY_VDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT-8),APPLY_VDEADBAND(GRAPHICS_BOTTOM));
/* frame */
drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM));
// Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) {
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
}
// Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) {
write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
}
}
void calcHomeArrow(int16_t m_yaw)
{
HomeLocationData home;
HomeLocationGet (&home);
GPSPositionData gpsData;
GPSPositionGet (&gpsData);
HomeLocationData home;
HomeLocationGet(&home);
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
/** http://www.movable-type.co.uk/scripts/latlong.html **/
/** http://www.movable-type.co.uk/scripts/latlong.html **/
float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
float elevation;
float gcsAlt=home.Altitude; // Home MSL altitude
float uavAlt=gpsData.Altitude; // UAV MSL altitude
float dAlt=uavAlt-gcsAlt; // Altitude difference
float gcsAlt = home.Altitude; // Home MSL altitude
float uavAlt = gpsData.Altitude; // UAV MSL altitude
float dAlt = uavAlt - gcsAlt; // Altitude difference
// Convert to radians
lat1 = DEG2RAD(home.Latitude)/10000000.0f; // Home lat
lon1 = DEG2RAD(home.Longitude)/10000000.0f; // Home lon
lat2 = DEG2RAD(gpsData.Latitude)/10000000.0f; // UAV lat
lon2 = DEG2RAD(gpsData.Longitude)/10000000.0f; // UAV lon
lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat
lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon
lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat
lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon
// Bearing
/**
var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg();
**/
y = sinf(lon2-lon1) * cosf(lat2);
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2-lon1);
var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg();
**/
y = sinf(lon2 - lon1) * cosf(lat2);
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
brng = RAD2DEG(atan2f(y,x));
if(brng<0)
brng+=360;
if (brng < 0) {
brng += 360.0f;
}
// yaw corrected bearing, needs compass
u2g=brng-180-m_yaw;
if(u2g<0)
u2g+=360;
u2g = brng - 180.0f - m_yaw;
if (u2g < 0) {
u2g += 360.0f;
}
// Haversine formula for distance
/**
var R = 6371; // km
var dLat = (lat2-lat1).toRad();
var dLon = (lon2-lon1).toRad();
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
Math.sin(dLon/2) * Math.sin(dLon/2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c;
**/
a = sinf((lat2-lat1)/2) * sinf((lat2-lat1)/2) +
cosf(lat1) * cosf(lat2) *
sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
d = 6371 * 1000 * c;
var R = 6371; // km
var dLat = (lat2-lat1).toRad();
var dLon = (lon2-lon1).toRad();
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
Math.sin(dLon/2) * Math.sin(dLon/2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c;
**/
a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a));
d = 6371.0f * 1000.0f * c;
// Elevation v depends servo direction
if(d > 0.0f)
elevation = 90-RAD2DEG(atanf(dAlt/d));
elevation = 90.0f - RAD2DEG(atanf(dAlt/d));
else
elevation = 0;
elevation = 0.0f;
//! TODO: sanity check
char temp[50]={0};
sprintf(temp,"hea:%d",(int)brng);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"ele:%d",(int)elevation);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"dis:%d",(int)d);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"u2g:%d",(int)u2g);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
char temp[50] =
{ 0 };
sprintf(temp, "hea:%d", (int) brng);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "ele:%d", (int) elevation);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "dis:%d", (int) d);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "u2g:%d", (int) u2g);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"%c%c",(int)(u2g/22.5f)*2+0x90,(int)(u2g/22.5f)*2+0x91);
write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91);
write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
}
int lama=10;
int lama = 10;
int lama_loc[2][30];
void lamas(void)
{
char temp[10]={0};
lama++;
if(lama%10==0)
{
for(int z=0; z<30;z++)
{
char temp[10] =
{ 0 };
lama++;
if (lama % 10 == 0) {
for (int z = 0; z < 30; z++) {
lama_loc[0][z]=rand()%(GRAPHICS_RIGHT-10);
lama_loc[1][z]=rand()%(GRAPHICS_BOTTOM-10);
}
}
for(int z=0; z<30;z++)
{
sprintf(temp,"%c",0xe8+(lama_loc[0][z]%2));
write_string(temp,APPLY_HDEADBAND(lama_loc[0][z]),APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
}
lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10);
lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10);
}
}
for (int z = 0; z < 30; z++) {
sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2));
write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
}
}
//main draw function
void updateGraphics() {
OsdSettingsData OsdSettings;
OsdSettingsGet (&OsdSettings);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
HomeLocationData home;
HomeLocationGet(&home);
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
switch (OsdSettings.Screen) {
case 0: // Dave simple
{
if(home.Set == HOMELOCATION_SET_FALSE)
{
char temps[20]={0};
sprintf(temps,"HOME NOT SET");
//printTextFB(x,y,temp);
write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM/2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
}
void updateGraphics()
{
OsdSettingsData OsdSettings;
OsdSettingsGet(&OsdSettings);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
HomeLocationData home;
HomeLocationGet(&home);
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
FlightStatusData status;
FlightStatusGet(&status);
char temp[50]={0};
memset(temp, ' ', 40);
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp,"V:%5.2fV",(PIOS_ADC_PinGet(2)*3*6.1f/4096));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
if(gpsData.Heading>180)
calcHomeArrow((int16_t)(gpsData.Heading-360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
}
break;
case 1:
{
/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
//write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
//write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
//drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
//drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
//drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
//drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
//drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
/*angleA++;
if(angleB<=-90)
{
sum=2;
}
if(angleB>=90)
{
sum=-2;
}
angleB+=sum;
angleC+=2;*/
// GPS HACK
if(gpsData.Heading>180)
calcHomeArrow((int16_t)(gpsData.Heading-360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
/* Draw Attitude Indicator */
if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED)
{
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96);
}
//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
//printText16( 60, 12,"Hello OP-OSD");
char temp[50]={0};
memset(temp, ' ', 40);
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Fix:%d",(int)gpsData.Status);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
/* Print RTC time */
if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED)
{
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]),APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
}
/* Print Number of detected video Lines */
sprintf(temp,"Lines:%4d",PIOS_Video_GetOSDLines());
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage */
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print CPU temperature */
sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(6)*0.29296875f-264));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage VIDEO*/
sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(3)*3.0f*6.1f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage RSSI */
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Draw Battery Gauge */
/*m_batt++;
uint8_t dir=3;
if(m_batt==101)
m_batt=0;
if(m_pitch>0)
{
dir=0;
m_alt+=m_pitch/2;
}
else if(m_pitch<0)
{
dir=1;
m_alt+=m_pitch/2;
}*/
PIOS_Servo_Set(0, OsdSettings.White);
PIOS_Servo_Set(1, OsdSettings.Black);
/*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
{
drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
}*/
//drawAltitude(200,50,m_alt,dir);
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.)
if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED)
{
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
}
// Draw altimeter (right side.)
if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED)
{
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
}
// Draw compass.
if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED)
{
if(attitude.Yaw<0) {
hud_draw_linear_compass(360+attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
} else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
}
}
}
break;
case 2:
{
int size=64;
int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2);
draw_artificial_horizon(-attitude.Roll,attitude.Pitch,APPLY_HDEADBAND(x),APPLY_VDEADBAND(y),size);
hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)),
APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
if(1)
{
hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
}
else
{
hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
}
switch (OsdSettings.Screen) {
case 0: // Dave simple
{
if (home.Set == HOMELOCATION_SET_FALSE) {
char temps[20] =
{ 0 };
sprintf(temps, "HOME NOT SET");
//printTextFB(x,y,temp);
write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
}
}
break;
case 3:
{
lamas();
}
break;
default:
write_vline_lm( APPLY_HDEADBAND(GRAPHICS_RIGHT/2),APPLY_VDEADBAND(0),APPLY_VDEADBAND(GRAPHICS_BOTTOM),1,1);
write_hline_lm( APPLY_HDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT),APPLY_VDEADBAND(GRAPHICS_BOTTOM/2),1,1);
break;
}
// Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) {
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
}
char temp[50] =
{ 0 };
memset(temp, ' ', 40);
// Note: cast to double required due to -Wdouble-promotion compiler option is
// being used, and there is no way in C to pass a float to a variadic function like sprintf()
sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
if (gpsData.Heading > 180)
calcHomeArrow((int16_t)(gpsData.Heading - 360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
}
break;
case 1:
{
/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
//write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
//write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
//drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
//drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
//drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
//drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
//drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
/*angleA++;
if(angleB<=-90)
{
sum=2;
}
if(angleB>=90)
{
sum=-2;
}
angleB+=sum;
angleC+=2;*/
// GPS HACK
if (gpsData.Heading > 180)
calcHomeArrow((int16_t)(gpsData.Heading - 360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
/* Draw Attitude Indicator */
if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96);
}
//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
//printText16( 60, 12,"Hello OP-OSD");
char temp[50] =
{ 0 };
memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Fix:%d", (int) gpsData.Status);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
/* Print RTC time */
if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
}
/* Print Number of detected video Lines */
sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines());
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage */
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print CPU temperature */
sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage VIDEO*/
sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage RSSI */
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Draw Battery Gauge */
/*m_batt++;
uint8_t dir=3;
if(m_batt==101)
m_batt=0;
if(m_pitch>0)
{
dir=0;
m_alt+=m_pitch/2;
}
else if(m_pitch<0)
{
dir=1;
m_alt+=m_pitch/2;
}*/
/*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
{
drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
}*/
//drawAltitude(200,50,m_alt,dir);
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.)
if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
}
// Draw altimeter (right side.)
if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
}
// Draw compass.
if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
if (attitude.Yaw < 0) {
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
} else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
}
}
}
break;
case 2:
{
int size = 64;
int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2);
draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size);
hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7,
10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) {
hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
} else {
hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500,
0);
}
char temp[50] =
{ 0 };
memset(temp, ' ', 50);
switch (status.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
sprintf(temp, "Man");
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
sprintf(temp, "Stab1");
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
sprintf(temp, "Stab2");
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
sprintf(temp, "Stab3");
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
sprintf(temp, "PH");
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
sprintf(temp, "RTB");
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
sprintf(temp, "PATH");
break;
default:
sprintf(temp, "Mode: %d", status.FlightMode);
break;
}
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
}
break;
case 3:
{
lamas();
}
break;
case 4:
case 5:
case 6:
{
int image = OsdSettings.Screen - 4;
struct splashEntry splash_info;
splash_info = splash[image];
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image);
}
break;
default:
write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1);
write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1);
break;
}
// Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) {
write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
}
}
void updateOnceEveryFrame() {
clearGraphics();
updateGraphics();
void updateOnceEveryFrame()
{
clearGraphics();
updateGraphics();
}
// ****************
/**
* Initialise the gps module
@ -2420,12 +2366,14 @@ void updateOnceEveryFrame() {
int32_t osdgenStart(void)
{
// Start gps task
vSemaphoreCreateBinary( osdSemaphore);
xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, osdgenTaskHandle);
return 0;
// Start gps task
vSemaphoreCreateBinary(osdSemaphore);
xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN);
#endif
return 0;
}
/**
@ -2435,69 +2383,77 @@ int32_t osdgenStart(void)
*/
int32_t osdgenInitialize(void)
{
AttitudeActualInitialize();
AttitudeActualInitialize();
#ifdef PIOS_INCLUDE_GPS
GPSPositionInitialize();
GPSPositionInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
GPSTimeInitialize();
GPSSatellitesInitialize();
#endif
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationInitialize();
HomeLocationInitialize();
#endif
#endif
OsdSettingsInitialize();
BaroAltitudeInitialize();
OsdSettingsInitialize();
BaroAltitudeInitialize();
FlightStatusInitialize();
return 0;
return 0;
}
MODULE_INITCALL(osdgenInitialize, osdgenStart)
MODULE_INITCALL( osdgenInitialize, osdgenStart)
// ****************
/**
* Main osd task. It does not return.
*/
static void osdgenTask(void *parameters)
static void osdgenTask(__attribute__((unused)) void *parameters)
{
//portTickType lastSysTime;
// Loop forever
//lastSysTime = xTaskGetTickCount();
//portTickType lastSysTime;
// Loop forever
//lastSysTime = xTaskGetTickCount();
OsdSettingsData OsdSettings;
OsdSettingsGet(&OsdSettings);
// intro
for(int i=0; i<63; i++)
{
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
{
clearGraphics();
introGraphics();
}
}
for(int i=0; i<63; i++)
{
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
{
clearGraphics();
introGraphics();
introText();
}
}
PIOS_Servo_Set(0, OsdSettings.White);
PIOS_Servo_Set(1, OsdSettings.Black);
while (1)
{
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
{
updateOnceEveryFrame();
// intro
for (int i = 0; i < 63; i++) {
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
clearGraphics();
introGraphics();
}
//xSemaphoreTake(osdSemaphore, portMAX_DELAY);
//vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
}
}
for (int i = 0; i < 63; i++) {
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
clearGraphics();
introGraphics();
introText();
}
}
while (1) {
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
#endif
updateOnceEveryFrame();
}
//xSemaphoreTake(osdSemaphore, portMAX_DELAY);
//vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

39
flight/modules/Osd/osdinput/inc/osdinput.h
View File

@ -1,15 +1,38 @@
/*
* OpOsd.h
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup osdinputModule osdinput Module
* @brief Process osdinput information
* @{
*
* Created on: 2.10.2011
* Author: Samba
* @file osdinput.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief osdinput module, handles osdinput stream
* @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 OPOSD_H_
#define OPOSD_H_
#ifndef OSDINPUT_H_
#define OSDINPUT_H_
#include "openpilot.h"
int32_t OpOsdInitialize(void);
int32_t osdinputInitialize(void);
#endif /* OPOSD_H_ */
#endif /* OSDINPUT_H_ */

295
flight/modules/Osd/osdinput/osdinput.c
View File

@ -36,261 +36,148 @@
#include "attitudeactual.h"
#include "taskinfo.h"
#include "flightstatus.h"
#include "fifo_buffer.h"
// ****************
// Private functions
static void OpOsdTask(void *parameters);
static void osdinputTask(void *parameters);
// ****************
// Private constants
#define GPS_TIMEOUT_MS 500
#define NMEA_MAX_PACKET_LENGTH 33 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
// same as in COM buffer
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 800
#else
#define STACK_SIZE_BYTES 1024
#endif
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define MAX_PACKET_LENGTH 33
// ****************
// Private variables
static uint32_t oposdPort;
static xTaskHandle OpOsdTaskHandle;
static xTaskHandle osdinputTaskHandle;
static char* oposd_rx_buffer;
t_fifo_buffer rx;
static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs;
static uint32_t numUpdates;
static uint32_t numChecksumErrors;
static uint32_t numParsingErrors;
enum osd_pkt_type
{
OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4,
};
// ****************
/**
* Initialise the gps module
* Initialise the osdinput module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OpOsdStart(void)
int32_t osdinputStart(void)
{
// Start gps task
xTaskCreate(OpOsdTask, (signed char *)"OSD", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &OpOsdTaskHandle);
//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, OpOsdTaskHandle);
// Start osdinput task
xTaskCreate(osdinputTask, (signed char *) "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle);
return 0;
return 0;
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OpOsdInitialize(void)
int32_t osdinputInitialize(void)
{
AttitudeActualInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.Roll = 0;
attitude.Pitch = 0;
attitude.Yaw = 0;
AttitudeActualSet(&attitude);
AttitudeActualInitialize();
FlightStatusInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.Roll = 0;
attitude.Pitch = 0;
attitude.Yaw = 0;
AttitudeActualSet(&attitude);
oposdPort = PIOS_COM_OSD;
// TODO: Get gps settings object
oposdPort = PIOS_COM_OSD;
oposd_rx_buffer = pvPortMalloc(MAX_PACKET_LENGTH);
PIOS_Assert(oposd_rx_buffer);
oposd_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
PIOS_Assert(oposd_rx_buffer);
return 0;
return 0;
}
MODULE_INITCALL(OpOsdInitialize, OpOsdStart)
MODULE_INITCALL( osdinputInitialize, osdinputStart)
// ****************
/**
* Main gps task. It does not return.
* Main osdinput task. It does not return.
*/
static void OpOsdTask(void *parameters)
static void osdinputTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
portTickType lastSysTime;
// Loop forever
lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
//GPSPositionData GpsData;
portTickType xDelay = 100 / portTICK_RATE_MS;
portTickType lastSysTime;
lastSysTime = xTaskGetTickCount();
//uint8_t rx_count = 0;
//bool start_flag = false;
//bool found_cr = false;
//int32_t gpsRxOverflow = 0;
uint8_t rx_count = 0;
bool start_flag = false;
int32_t osdRxOverflow = 0;
uint8_t c = 0xAA;
// Loop forever
while (1) {
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) {
numUpdates = 0;
numChecksumErrors = 0;
numParsingErrors = 0;
// detect start while acquiring stream
if (!start_flag && ((c == 0xCB) || (c == 0x34))) {
start_flag = true;
rx_count = 0;
} else if (!start_flag) {
continue;
}
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
uint8_t rx_count = 0;
bool start_flag = false;
//bool found_cr = false;
int32_t gpsRxOverflow = 0;
uint8_t c=0xAA;
// Loop forever
while (1)
{
/*//DMA_Cmd(DMA1_Stream2, DISABLE); //prohibit channel3 for a little time
uint16_t cnt = DMA_GetCurrDataCounter(DMA1_Stream2);
rx.wr = rx.buf_size-cnt;
if(rx.wr)
{
//PIOS_LED_Toggle(LED2);
while ( fifoBuf_getData(&rx, &c, 1) > 0)
{
// detect start while acquiring stream
if (!start_flag && ((c == 0xCB) || (c == 0x34)))
{
start_flag = true;
rx_count = 0;
}
else
if (!start_flag)
continue;
if (rx_count >= 11)
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = false;
rx_count = 0;
}
else
{
oposd_rx_buffer[rx_count] = c;
rx_count++;
}
if (start_flag && rx_count == 11)
{
//PIOS_LED_Toggle(LED3);
if(oposd_rx_buffer[1]==3)
{
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8);
attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8);
attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8);
AttitudeActualSet(&attitude);
//setAttitudeOsd((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8), //pitch
// (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8), //roll
// (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8)); //yaw
}
//frame completed
start_flag = false;
rx_count = 0;
}
}
}
//DMA_Cmd(DMA1_Stream2, ENABLE);
*/
//PIOS_COM_SendBufferNonBlocking(oposdPort, &c, 1);
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0)
{
// detect start while acquiring stream
if (!start_flag && ((c == 0xCB) || (c == 0x34)))
{
start_flag = true;
rx_count = 0;
}
else
if (!start_flag)
continue;
if (rx_count >= 11)
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = false;
rx_count = 0;
}
else
{
oposd_rx_buffer[rx_count] = c;
rx_count++;
}
if (rx_count == 11)
{
if(oposd_rx_buffer[1]==3)
{
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8);
attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8);
attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8);
AttitudeActualSet(&attitude);
}
//frame completed
start_flag = false;
rx_count = 0;
}
}
vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
{ // we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
}
else
{ // we appear to be receiving GPS sentences OK, we've had an update
}
}
if (rx_count >= 11) {
// Flush the buffer and note the overflow event.
osdRxOverflow++;
start_flag = false;
rx_count = 0;
} else {
oposd_rx_buffer[rx_count] = c;
rx_count++;
}
if (rx_count == 11) {
if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) {
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f;
attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f;
attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f;
AttitudeActualSet(&attitude);
} else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) {
FlightStatusData status;
FlightStatusGet(&status);
status.Armed = oposd_rx_buffer[8];
status.FlightMode = oposd_rx_buffer[3];
FlightStatusSet(&status);
}
//frame completed
start_flag = false;
rx_count = 0;
}
}
vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

40
flight/modules/Osd/osdoutput/inc/osdoutput.h Normal file
View File

@ -0,0 +1,40 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup osdoutputModule osdoutput Module
* @brief Process osdoutput information
* @{
*
* @file osdoutput.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief osdoutput module, handles osdoutput stream
* @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 OSDOUTPUT_H
#define OSDOUTPUT_H
int32_t osdoutputInitialize(void);
#endif /* OSDOUTPUT_H */
/**
* @}
* @}
*/

304
flight/modules/Osd/osdoutput/osdoutput.c Normal file
View File

@ -0,0 +1,304 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup OSDOUTPUTModule OSDOutput Module
* @brief On screen display support
* @{
*
* @file osdoutput.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Interfacing with OSD 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"
#if FLIGHTBATTERYSTATE_SUPPORTED
#include "flightbatterystate.h"
#endif
#if POSITIONACTUAL_SUPPORTED
#include "positionactual.h"
#endif
#include "systemalarms.h"
#include "attitudeactual.h"
#include "hwsettings.h"
#include "flightstatus.h"
static bool osdoutputEnabled;
enum osd_hk_sync
{
OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34,
};
enum osd_hk_pkt_type
{
OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4,
};
enum osd_hk_control_mode
{
OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2,
};
struct osd_hk_blob_misc
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */
int16_t roll;
int16_t pitch;
//uint16_t home; /* Big Endian */
enum osd_hk_control_mode control_mode;
uint8_t low_battery;
uint16_t current; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_att
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */
int16_t roll;
int16_t pitch;
int16_t yaw;
int16_t speed; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_nav
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */
uint32_t gps_lat; /* Big Endian */
uint32_t gps_lon; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_maint
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */
uint8_t gps_speed;
uint16_t gps_alt; /* Big Endian */
uint16_t gps_dis; /* Big Endian */
uint8_t status;
uint8_t config;
uint8_t emerg;
}__attribute__((packed));
struct osd_hk_blob_mode
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */
uint8_t fltmode;
uint16_t gps_alt; /* Big Endian */
uint16_t gps_dis; /* Big Endian */
uint8_t armed;
uint8_t config;
uint8_t emerg;
}__attribute__((packed));
union osd_hk_pkt_blobs
{
struct osd_hk_blob_misc misc;
struct osd_hk_blob_nav nav;
struct osd_hk_blob_maint maint;
struct osd_hk_blob_att att;
struct osd_hk_blob_mode mode;
}__attribute__((packed));
struct osd_hk_msg
{
enum osd_hk_sync sync;
enum osd_hk_pkt_type t;
union osd_hk_pkt_blobs v;
}__attribute__((packed));
static struct osd_hk_msg osd_hk_msg_buf;
static volatile bool newPositionActualData = false;
static volatile bool newBattData = false;
static volatile bool newAttitudeData = false;
static volatile bool newAlarmData = false;
static uint32_t osd_hk_com_id;
static uint8_t osd_hk_msg_dropped;
static uint8_t osd_packet;
static void send_update(__attribute__((unused)) UAVObjEvent * ev)
{
static enum osd_hk_sync sync = OSD_HK_SYNC_A;
struct osd_hk_msg * msg = &osd_hk_msg_buf;
union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v);
/* Make sure we have a COM port bound */
if (!osd_hk_com_id) {
return;
}
FlightStatusData flightStatus;
/*
* Set up the message
*/
msg->sync = sync;
switch (osd_packet) {
case OSD_HK_PKT_TYPE_MISC:
break;
case OSD_HK_PKT_TYPE_NAV:
break;
case OSD_HK_PKT_TYPE_MAINT:
break;
case OSD_HK_PKT_TYPE_ATT:
msg->t = OSD_HK_PKT_TYPE_ATT;
float roll;
AttitudeActualRollGet(&roll);
blob->att.roll = (int16_t)(roll * 10);
float pitch;
AttitudeActualPitchGet(&pitch);
blob->att.pitch = (int16_t)(pitch * 10);
float yaw;
AttitudeActualYawGet(&yaw);
blob->att.yaw = (int16_t)(yaw * 10);
break;
case OSD_HK_PKT_TYPE_MODE:
msg->t = OSD_HK_PKT_TYPE_MODE;
FlightStatusGet(&flightStatus);
blob->mode.fltmode = flightStatus.FlightMode;
blob->mode.armed = flightStatus.Armed;
break;
default:
break;
}
/* Field not supported yet */
//blob->misc.control_mode = 0;
/*if (newAlarmData) {
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) {
case SYSTEMALARMS_ALARM_UNINITIALISED:
case SYSTEMALARMS_ALARM_OK:
blob->misc.low_battery = 0;
break;
case SYSTEMALARMS_ALARM_WARNING:
case SYSTEMALARMS_ALARM_ERROR:
case SYSTEMALARMS_ALARM_CRITICAL:
default:
blob->misc.low_battery = 1;
break;
}
newAlarmData = false;
}*/
#if FLIGHTBATTERYSUPPORTED
if (newBattData) {
float consumed_energy;
FlightBatteryStateConsumedEnergyGet(&consumed_energy);
uint16_t current = (uint16_t)(consumed_energy * 10);
/* convert to big endian */
blob->misc.current = (
(current & 0xFF00 >> 8) |
(current & 0x00FF << 8));
newBattData = false;
}
#else
//blob->misc.current = 0;
#endif
#if POSITIONACTUAL_SUPPORTED
if (newPositionActualData) {
PositionActualData position;
PositionActualGet(&position);
/* compute 3D distance */
float d = sqrt(
pow(position.North,2) +
pow(position.East,2) +
pow(position.Down,2));
/* convert from cm to dm (10ths of m) */
uint16_t home = (uint16_t)(d / 10);
/* convert to big endian */
blob->misc.home = (
(home & 0xFF00 >> 8) |
(home & 0x00FF << 8));
newPositionActualData = false;
}
#else
//blob->misc.home = 0;
#endif
if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) {
/* Sent a packet, flip to the opposite sync */
if (sync == OSD_HK_SYNC_A) {
sync = OSD_HK_SYNC_B;
} else {
sync = OSD_HK_SYNC_A;
}
} else {
/* Failed to send this update */
osd_hk_msg_dropped++;
}
osd_packet++;
if (osd_packet > OSD_HK_PKT_TYPE_MODE) {
osd_packet = OSD_HK_PKT_TYPE_MISC;
}
}
static UAVObjEvent ev;
static int32_t osdoutputStart(void)
{
if (osdoutputEnabled) {
/* Start a periodic timer to kick sending of an update */
EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS);
return 0;
}
return -1;
}
static int32_t osdoutputInitialize(void)
{
osd_hk_com_id = PIOS_COM_OSDHK;
#ifdef MODULE_OSDOUTPUT_BUILTIN
osdoutputEnabled = 1;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
osdoutputEnabled = 1;
} else {
osdoutputEnabled = 0;
}
#endif
return 0;
}
MODULE_INITCALL(osdoutputInitialize, osdoutputStart)
/**
* @}
* @}
*/

2
flight/modules/OveroSync/overosync.c
View File

@ -162,7 +162,7 @@ static void registerObject(UAVObjHandle obj)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
static void overoSyncTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;

2
flight/modules/OveroSync/simulated/overosync.c
View File

@ -161,7 +161,7 @@ static void registerObject(UAVObjHandle obj)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
static void overoSyncTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;

19
flight/modules/PathPlanner/pathplanner.c
View File

@ -116,7 +116,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
static void pathPlannerTask(void *parameters)
static void pathPlannerTask(__attribute__((unused)) void *parameters)
{
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
@ -208,7 +208,7 @@ static void pathPlannerTask(void *parameters)
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(UAVObjEvent * ev) {
void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) {
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) return;
@ -238,20 +238,25 @@ void updatePathDesired(UAVObjEvent * ev) {
pathDesired.UID = waypointActive.Index;
if(waypointActive.Index == 0) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);

160
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -71,8 +71,9 @@ typedef struct {
// The task handles.
xTaskHandle telemetryTxTaskHandle;
xTaskHandle radioRxTaskHandle;
xTaskHandle telemetryRxTaskHandle;
xTaskHandle radioTxTaskHandle;
xTaskHandle radioRxTaskHandle;
// The UAVTalk connection on the com side.
UAVTalkConnection outUAVTalkCon;
@ -91,6 +92,9 @@ typedef struct {
// Should we parse UAVTalk?
bool parseUAVTalk;
// We can only configure the hardware once.
bool configured;
// The current configured uart speed
OPLinkSettingsComSpeedOptions comSpeed;
@ -100,8 +104,9 @@ typedef struct {
// Private functions
static void telemetryTxTask(void *parameters);
static void radioRxTask(void *parameters);
static void telemetryRxTask(void *parameters);
static void radioTxTask(void *parameters);
static void radioRxTask(void *parameters);
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte);
@ -136,59 +141,65 @@ static int32_t RadioComBridgeStart(void)
bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id);
if (is_coordinator) {
// Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) {
case OPLINKSETTINGS_MAXRFPOWER_125:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
break;
case OPLINKSETTINGS_MAXRFPOWER_16:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
break;
case OPLINKSETTINGS_MAXRFPOWER_316:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
break;
case OPLINKSETTINGS_MAXRFPOWER_63:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
break;
case OPLINKSETTINGS_MAXRFPOWER_126:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
break;
case OPLINKSETTINGS_MAXRFPOWER_25:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
break;
case OPLINKSETTINGS_MAXRFPOWER_50:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
break;
case OPLINKSETTINGS_MAXRFPOWER_100:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
break;
default:
// do nothing
break;
}
// Set the frequency range.
PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing);
// Reinitilize the modem.
PIOS_RFM22B_Reinit(pios_rfm22b_id);
// We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
(oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
}
// Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) {
case OPLINKSETTINGS_MAXRFPOWER_125:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
break;
case OPLINKSETTINGS_MAXRFPOWER_16:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
break;
case OPLINKSETTINGS_MAXRFPOWER_316:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
break;
case OPLINKSETTINGS_MAXRFPOWER_63:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
break;
case OPLINKSETTINGS_MAXRFPOWER_126:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
break;
case OPLINKSETTINGS_MAXRFPOWER_25:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
break;
case OPLINKSETTINGS_MAXRFPOWER_50:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
break;
case OPLINKSETTINGS_MAXRFPOWER_100:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
break;
default:
// do nothing
break;
}
// Set the initial frequency.
PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency);
// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
xTaskCreate(telemetryTxTask, (signed char *)"telemTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle));
xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle));
xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle));
xTaskCreate(telemetryRxTask, (signed char *)"telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle));
xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle));
xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle));
// Register the watchdog timers.
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRY);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX);
PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX);
PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX);
#endif
return 0;
}
@ -231,7 +242,8 @@ static int32_t RadioComBridgeInitialize(void)
data->comTxErrors = 0;
data->comTxRetries = 0;
data->UAVTalkErrors = 0;
data->parseUAVTalk = false;
data->parseUAVTalk = true;
data->configured = false;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B;
@ -244,14 +256,14 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
*
* @param[in] parameters The task parameters
*/
static void telemetryTxTask(void *parameters)
static void telemetryTxTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRY);
PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYTX);
#endif
// Wait for queue message
if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
@ -290,12 +302,40 @@ static void telemetryTxTask(void *parameters)
}
}
/**
* Radio tx task. Receive data packets from the com port and send to the radio.
*
* @param[in] parameters The task parameters
*/
static void radioTxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX);
#endif
// Wait until the com port is available.
if (data->parseUAVTalk || !PIOS_COM_TELEMETRY) {
vTaskDelay(5);
continue;
}
// Read from the com port.
uint8_t serial_data[1];
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEMETRY, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
if (bytes_to_process > 0) {
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, serial_data, bytes_to_process);
}
}
}
/**
* Radio rx task. Receive data packets from the radio and pass them on.
*
* @param[in] parameters The task parameters
*/
static void radioRxTask(void *parameters)
static void radioRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
@ -305,27 +345,32 @@ static void radioRxTask(void *parameters)
uint8_t serial_data[1];
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
// Either pass the data through the UAVTalk parser, or just send it to the radio (if we're doing raw comms).
if (data->parseUAVTalk) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
}
}
} else if (PIOS_COM_TELEMETRY) {
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
}
}
}
}
/**
* Radio rx task. Receive data from a com port and pass it on to the radio.
* Receive telemetry from the USB/COM port.
*
* @param[in] parameters The task parameters
*/
static void radioTxTask(void *parameters)
static void telemetryRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
uint32_t inputPort = PIOS_COM_TELEMETRY;
uint32_t inputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX);
PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYRX);
#endif
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
@ -356,7 +401,7 @@ static void radioTxTask(void *parameters)
*/
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
{
uint32_t outputPort = PIOS_COM_TELEMETRY;
uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) {
@ -573,6 +618,11 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port,
// Set the frequency range.
PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing);
// We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
(oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
// Update the OPLinkSettings object.
OPLinkSettingsSet(&oplinkSettings);
}
@ -590,11 +640,16 @@ static void updateSettings()
OPLinkSettingsData oplinkSettings;
OPLinkSettingsGet(&oplinkSettings);
// We can only configure the hardware once.
if (data->configured) {
return;
}
data->configured = true;
// Configure the main port
bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id);
switch (oplinkSettings.MainPort) {
case OPLINKSETTINGS_MAINPORT_TELEMETRY:
data->parseUAVTalk = true;
case OPLINKSETTINGS_MAINPORT_SERIAL:
/* Configure the main port for uart serial */
PIOS_InitUartMainPort();
@ -610,7 +665,6 @@ static void updateSettings()
// Configure the flexi port
switch (oplinkSettings.FlexiPort) {
case OPLINKSETTINGS_FLEXIPORT_TELEMETRY:
data->parseUAVTalk = true;
case OPLINKSETTINGS_FLEXIPORT_SERIAL:
/* Configure the flexi port as uart serial */
PIOS_InitUartFlexiPort();

20
flight/modules/Sensors/sensors.c
View File

@ -88,6 +88,8 @@ static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0};
static float gyro_staticbias[3] = {0,0,0};
static float gyro_scale[3] = {0,0,0};
static float R[3][3] = {{0}};
static int8_t rotate = 0;
@ -156,7 +158,7 @@ int32_t mag_test;
*/
uint32_t sensor_dt_us;
static void SensorsTask(void *parameters)
static void SensorsTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
uint32_t accel_samples = 0;
@ -358,9 +360,9 @@ static void SensorsTask(void *parameters)
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};
float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
@ -524,7 +526,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
/**
* Locally cache some variables from the AtttitudeSettings object
*/
static void settingsUpdatedCb(UAVObjEvent * objEv) {
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@ -539,8 +541,12 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
// Do not store gyros_bias here as that comes from the state estimator and should be
// used from GyroBias directly
gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
// Zero out any adaptive tracking
MagBiasData magBias;

2
flight/modules/Sensors/simulated/sensors.c
View File

@ -145,7 +145,7 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart)
* Simulated sensor task. Run a model of the airframe and produce sensor values
*/
int sensors_count;
static void SensorsTask(void *parameters)
static void SensorsTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;

2
flight/modules/Stabilization/inc/relay_tuning.h
View File

@ -36,4 +36,4 @@
int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
#endif
#endif

9
flight/modules/Stabilization/relay_tuning.c
View File

@ -32,15 +32,11 @@
*/
#include "openpilot.h"
#include "stabilization.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
@ -108,7 +104,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
if(phase >= 360)
phase = 0;
accum_sin += sin_lookup_deg(phase) * error;
accum_cos += sin_lookup_deg(phase + 90) * error;
accum_cos += cos_lookup_deg(phase) * error;
accumulated ++;
// Make sure we've had enough time since last transition then check for a change in the output
@ -148,4 +144,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
return 0;
}

19
flight/modules/Stabilization/stabilization.c
View File

@ -55,6 +55,9 @@
#include "relay_tuning.h"
#include "virtualflybar.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
// Private constants
#define MAX_QUEUE_SIZE 1
@ -125,7 +128,6 @@ int32_t StabilizationInitialize()
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
#endif
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
@ -139,7 +141,7 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
/**
* Module task
*/
static void stabilizationTask(void* parameters)
static void stabilizationTask(__attribute__((unused)) void* parameters)
{
UAVObjEvent ev;
@ -213,7 +215,12 @@ static void stabilizationTask(void* parameters)
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw};
local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if(modulo<0)
local_error[2] = modulo + 180.0f;
else
local_error[2] = modulo - 180.0f;
#endif
float gyro_filtered[3];
@ -279,6 +286,7 @@ static void stabilizationTask(void* parameters)
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
if (reinit)
@ -294,6 +302,7 @@ static void stabilizationTask(void* parameters)
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
@ -309,7 +318,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -425,7 +434,7 @@ static float bound(float val, float range)
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev)
{
StabilizationSettingsGet(&settings);

596
flight/modules/System/systemmod.c
View File

@ -72,8 +72,8 @@
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif
#if defined(PIOS_SYSTEM_STACK_SIZE)
@ -114,15 +114,15 @@ static void updateWDGstats();
*/
int32_t SystemModStart(void)
{
// Initialize vars
stackOverflow = false;
mallocFailed = false;
// Create system task
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
// Initialize vars
stackOverflow = false;
mallocFailed = false;
// Create system task
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
return 0;
}
/**
@ -132,61 +132,64 @@ int32_t SystemModStart(void)
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
ObjectPersistenceInitialize();
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
ObjectPersistenceInitialize();
#ifdef DIAG_TASKS
TaskInfoInitialize();
TaskInfoInitialize();
#endif
#ifdef DIAG_I2C_WDG_STATS
I2CStatsInitialize();
WatchdogStatusInitialize();
I2CStatsInitialize();
WatchdogStatusInitialize();
#endif
objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
if (objectPersistenceQueue == NULL)
return -1;
objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
if (objectPersistenceQueue == NULL)
return -1;
SystemModStart();
SystemModStart();
return 0;
return 0;
}
MODULE_INITCALL(SystemModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
static void systemTask(__attribute__((unused))void *parameters)
{
/* start the delayed callback scheduler */
CallbackSchedulerStart();
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
// Load a copy of HwSetting active at boot time
HwSettingsGet(&bootHwSettings);
// Whenever the configuration changes, make sure it is safe to fly
HwSettingsConnectCallback(hwSettingsUpdatedCb);
// Whenever the configuration changes, make sure it is safe to fly
HwSettingsConnectCallback(hwSettingsUpdatedCb);
#ifdef DIAG_TASKS
TaskInfoData taskInfoData;
@ -197,11 +200,11 @@ static void systemTask(void *parameters)
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
// Update the system alarms
updateSystemAlarms();
#ifdef DIAG_I2C_WDG_STATS
updateI2Cstats();
updateWDGstats();
updateI2Cstats();
updateWDGstats();
#endif
#ifdef DIAG_TASKS
@ -210,34 +213,34 @@ static void systemTask(void *parameters)
TaskInfoSet(&taskInfoData);
#endif
// Flash the heartbeat LED
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
// Turn on the error LED if an alarm is set
#if defined (PIOS_LED_ALARM)
if (AlarmsHasWarnings()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
if (AlarmsHasWarnings()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
#endif /* PIOS_LED_ALARM */
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
}
/**
@ -245,108 +248,101 @@ static void systemTask(void *parameters)
*/
static void objectUpdatedCb(UAVObjEvent * ev)
{
ObjectPersistenceData objper;
UAVObjHandle obj;
ObjectPersistenceData objper;
UAVObjHandle obj;
// If the object updated was the ObjectPersistence execute requested action
if (ev->obj == ObjectPersistenceHandle()) {
// Get object data
ObjectPersistenceGet(&objper);
// If the object updated was the ObjectPersistence execute requested action
if (ev->obj == ObjectPersistenceHandle()) {
// Get object data
ObjectPersistenceGet(&objper);
int retval = 1;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
int retval = 1;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// When this is called because of this method don't do anything
if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR ||
objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
return;
}
// When this is called because of this method don't do anything
if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR || objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
return;
}
// Execute action if disarmed
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
retval = -1;
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Load selected instance
retval = UAVObjLoad(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjLoadSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjLoadMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Save selected instance
retval = UAVObjSave(obj, objper.InstanceID);
// Execute action if disarmed
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
retval = -1;
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Load selected instance
retval = UAVObjLoad(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjLoadSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjLoadMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Save selected instance
retval = UAVObjSave(obj, objper.InstanceID);
// Not sure why this is needed
vTaskDelay(10);
// Not sure why this is needed
vTaskDelay(10);
// Verify saving worked
if (retval == 0)
retval = UAVObjLoad(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjSaveSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjSaveMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Delete selected instance
retval = UAVObjDelete(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjDeleteSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
|| objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjDeleteMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
// Verify saving worked
if (retval == 0)
retval = UAVObjLoad(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjSaveSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjSaveMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
// Get selected object
obj = UAVObjGetByID(objper.ObjectID);
if (obj == 0) {
return;
}
// Delete selected instance
retval = UAVObjDelete(obj, objper.InstanceID);
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjDeleteSettings();
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
retval = UAVObjDeleteMetaobjects();
}
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
retval = PIOS_FLASHFS_Format(0);
retval = PIOS_FLASHFS_Format(0);
#else
retval = -1;
#endif
}
switch(retval) {
case 0:
objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
ObjectPersistenceSet(&objper);
break;
case -1:
objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
ObjectPersistenceSet(&objper);
break;
default:
break;
}
}
}
switch (retval) {
case 0:
objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
ObjectPersistenceSet(&objper);
break;
case -1:
objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
ObjectPersistenceSet(&objper);
break;
default:
break;
}
}
}
/**
* Called whenever hardware settings changed
*/
static void hwSettingsUpdatedCb(UAVObjEvent * ev)
static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev)
{
HwSettingsData currentHwSettings;
HwSettingsGet(&currentHwSettings);
@ -373,76 +369,75 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
* Called periodically to update the I2C statistics
*/
#ifdef DIAG_I2C_WDG_STATS
static void updateI2Cstats()
static void updateI2Cstats()
{
#if defined(PIOS_INCLUDE_I2C)
I2CStatsData i2cStats;
I2CStatsGet(&i2cStats);
struct pios_i2c_fault_history history;
PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
i2cStats.evirq_log[i] = history.evirq[i];
i2cStats.erirq_log[i] = history.erirq[i];
i2cStats.event_log[i] = history.event[i];
i2cStats.state_log[i] = history.state[i];
}
i2cStats.last_error_type = history.type;
I2CStatsSet(&i2cStats);
I2CStatsData i2cStats;
I2CStatsGet(&i2cStats);
struct pios_i2c_fault_history history;
PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
i2cStats.evirq_log[i] = history.evirq[i];
i2cStats.erirq_log[i] = history.erirq[i];
i2cStats.event_log[i] = history.event[i];
i2cStats.state_log[i] = history.state[i];
}
i2cStats.last_error_type = history.type;
I2CStatsSet(&i2cStats);
#endif
}
static void updateWDGstats()
static void updateWDGstats()
{
WatchdogStatusData watchdogStatus;
watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
WatchdogStatusSet(&watchdogStatus);
WatchdogStatusData watchdogStatus;
watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
WatchdogStatusSet(&watchdogStatus);
}
#endif
/**
* Called periodically to update the system stats
*/
static uint16_t GetFreeIrqStackSize(void)
{
uint32_t i = 0x200;
uint32_t i = 0x200;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
extern uint32_t _irq_stack_top;
extern uint32_t _irq_stack_end;
uint32_t pattern = 0x0000A5A5;
uint32_t *ptr = &_irq_stack_end;
extern uint32_t _irq_stack_top;
extern uint32_t _irq_stack_end;
uint32_t pattern = 0x0000A5A5;
uint32_t *ptr = &_irq_stack_end;
#if 1 /* the ugly way accurate but takes more time, useful for debugging */
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
for (i=0; i< stack_size; i++)
{
if (ptr[i] != pattern)
{
i=i*4;
break;
}
}
for (i=0; i< stack_size; i++)
{
if (ptr[i] != pattern)
{
i=i*4;
break;
}
}
#else /* faster way but not accurate */
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
{
i = IRQSTACK_LIMIT_CRITICAL - 1;
}
else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
{
i = IRQSTACK_LIMIT_WARNING - 1;
}
else
{
i = IRQSTACK_LIMIT_WARNING;
}
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
{
i = IRQSTACK_LIMIT_CRITICAL - 1;
}
else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
{
i = IRQSTACK_LIMIT_WARNING - 1;
}
else
{
i = IRQSTACK_LIMIT_WARNING;
}
#endif
#endif
return i;
return i;
}
/**
@ -450,43 +445,49 @@ uint32_t *ptr = &_irq_stack_end;
*/
static void updateStats()
{
static portTickType lastTickCount = 0;
SystemStatsData stats;
static portTickType lastTickCount = 0;
SystemStatsData stats;
// Get stats and update
SystemStatsGet(&stats);
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
// Get stats and update
SystemStatsGet(&stats);
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
stats.HeapRemaining = 10240;
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
stats.HeapRemaining = 10240;
#else
stats.HeapRemaining = xPortGetFreeHeapSize();
stats.HeapRemaining = xPortGetFreeHeapSize();
#endif
// Get Irq stack status
stats.IRQStackRemaining = GetFreeIrqStackSize();
// Get Irq stack status
stats.IRQStackRemaining = GetFreeIrqStackSize();
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
if (idleCounterClear) {
idleCounter = 0;
}
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
if (idleCounterClear) {
idleCounter = 0;
}
portTickType now = xTaskGetTickCount();
if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
} //else: TickCount has wrapped, do not calc now
lastTickCount = now;
idleCounterClear = 1;
portTickType now = xTaskGetTickCount();
if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
stats.CPULoad =
100 - (uint8_t) roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
} //else: TickCount has wrapped, do not calc now
lastTickCount = now;
idleCounterClear = 1;
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((float)((1 << 12) - 1));
const float STM32_TEMP_V25 = 1.43f; /* V */
const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
#if defined(STM32F4XX)
float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
const float STM32_TEMP_V25 = 0.76f; /* V */
const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
#else
float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
const float STM32_TEMP_V25 = 1.43f; /* V */
const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
#endif
SystemStatsSet(&stats);
#endif
SystemStatsSet(&stats);
}
/**
@ -494,66 +495,64 @@ static void updateStats()
*/
static void updateSystemAlarms()
{
SystemStatsData stats;
UAVObjStats objStats;
EventStats evStats;
SystemStatsGet(&stats);
SystemStatsData stats;
UAVObjStats objStats;
EventStats evStats;
SystemStatsGet(&stats);
// Check heap, IRQ stack and malloc failures
if ( mallocFailed
|| (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
// Check heap, IRQ stack and malloc failures
if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
#endif
) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
} else if (
(stats.HeapRemaining < HEAP_LIMIT_WARNING)
) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
} else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING)
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
#endif
) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
}
) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
}
// Check CPU load
if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
} else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
}
// Check CPU load
if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
} else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
}
// Check for stack overflow
if (stackOverflow) {
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
}
// Check for stack overflow
if (stackOverflow) {
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
}
// Check for event errors
UAVObjGetStats(&objStats);
EventGetStats(&evStats);
UAVObjClearStats();
EventClearStats();
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
}
if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
SystemStatsData sysStats;
SystemStatsGet(&sysStats);
sysStats.EventSystemWarningID = evStats.lastErrorID;
sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
SystemStatsSet(&sysStats);
}
// Check for event errors
UAVObjGetStats(&objStats);
EventGetStats(&evStats);
UAVObjClearStats();
EventClearStats();
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
}
if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
SystemStatsData sysStats;
SystemStatsGet(&sysStats);
sysStats.EventSystemWarningID = evStats.lastErrorID;
sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
SystemStatsSet(&sysStats);
}
}
/**
@ -561,26 +560,27 @@ static void updateSystemAlarms()
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
/**
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
void vApplicationStackOverflowHook(__attribute__((unused))xTaskHandle * pxTask,
__attribute__((unused))signed portCHAR * pcTaskName)
{
stackOverflow = true;
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
@ -590,15 +590,15 @@ void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTas
#define DEBUG_MALLOC_FAILURES 0
void vApplicationMallocFailedHook(void)
{
mallocFailed = true;
mallocFailed = true;
#if DEBUG_MALLOC_FAILURES
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
/**
* @}
* @}
*/
* @}
* @}
*/

6
flight/modules/Telemetry/telemetry.c
View File

@ -307,7 +307,7 @@ static void processObjEvent(UAVObjEvent * ev)
/**
* Telemetry transmit task, regular priority
*/
static void telemetryTxTask(void *parameters)
static void telemetryTxTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
@ -325,7 +325,7 @@ static void telemetryTxTask(void *parameters)
* Telemetry transmit task, high priority
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
static void telemetryTxPriTask(void *parameters)
static void telemetryTxPriTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
@ -343,7 +343,7 @@ static void telemetryTxPriTask(void *parameters)
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
static void telemetryRxTask(__attribute__((unused)) void *parameters)
{
// Task loop

947
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -73,6 +73,11 @@
#include "paths.h"
#include "CoordinateConversions.h"
#include "cameradesired.h"
#include "poilearnsettings.h"
#include "poilocation.h"
#include "accessorydesired.h"
// Private constants
#define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548
@ -82,19 +87,22 @@
// Private variables
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static PathStatusData pathStatus;
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
static float poiRadius;
// Private functions
static void vtolPathFollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updateNedAccel();
static void updatePOIBearing();
static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float* attitude);
static void updateVtolDesiredAttitude();
static void updateVtolDesiredAttitude(bool yaw_attitude);
static float bound(float val, float min, float max);
static bool vtolpathfollower_enabled;
static void accessoryUpdated(UAVObjEvent* ev);
/**
* Initialise the module, called on startup
@ -102,13 +110,13 @@ static bool vtolpathfollower_enabled;
*/
int32_t VtolPathFollowerStart()
{
if (vtolpathfollower_enabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
if (vtolpathfollower_enabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
return 0;
}
/**
@ -117,25 +125,29 @@ int32_t VtolPathFollowerStart()
*/
int32_t VtolPathFollowerInitialize()
{
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
}
return 0;
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
CameraDesiredInitialize();
AccessoryDesiredInitialize();
PoiLearnSettingsInitialize();
PoiLocationInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
}
return 0;
}
MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart)
MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart)
static float northVelIntegral = 0;
static float eastVelIntegral = 0;
@ -149,118 +161,205 @@ static float throttleOffset = 0;
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
PathStatusData pathStatus;
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
portTickType lastUpdateTime;
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
AccessoryDesiredConnectCallback(accessoryUpdated);
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch(pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
updateEndpointVelocity();
updateVtolDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
break;
}
// Convert the accels into the NED frame
updateNedAccel();
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
}
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
}
/**
* Compute bearing and elevation between current position and POI
*/
static void updatePOIBearing()
{
const float DEADBAND_HIGH = 0.10f;
const float DEADBAND_LOW = -0.10f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
PositionActualGet(&positionActual);
CameraDesiredData cameraDesired;
CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionActual.North - poi.North;
dLoc[1] = positionActual.East - poi.East;
dLoc[2] = positionActual.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
//not above
if (distance >= 3.0f) {
//You can feed this into camerastabilization
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
//cameraDesired.Yaw=yaw;
//cameraDesired.PitchOrServo2=elevation;
CameraDesiredSet(&cameraDesired);
StabilizationDesiredSet(&stabDesired);
}
}
/**
@ -271,51 +370,74 @@ static void vtolPathFollowerTask(void *parameters)
*/
static void updatePathVelocity()
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand;
PositionActualData positionActual;
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
if(progress.fractional_progress > 1)
groundspeed = 0;
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
float scale = 1;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
PositionActualGet(&positionActual);
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
float cur[3] =
{ positionActual.North, positionActual.East, positionActual.Down };
struct path_status progress;
float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
VelocityDesiredSet(&velocityDesired);
float groundspeed;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1)
groundspeed = 0;
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1)
groundspeed = 0;
break;
}
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] =
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
}
/**
@ -326,38 +448,41 @@ static void updatePathVelocity()
*/
void updateEndpointVelocity()
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PositionActualData positionActual;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float downCommand;
float northPos = 0, eastPos = 0, downPos = 0;
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float downCommand;
float northPos = 0, eastPos = 0, downPos = 0;
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
float alt = homeLocation.Altitude;
float alt = homeLocation.Altitude;
float T[3] = { alt+6.378137E6f,
cosf(lat)*(alt+6.378137E6f),
-1.0f};
@ -365,50 +490,46 @@ void updateEndpointVelocity()
T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)),
T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))};
northPos = NED[0];
eastPos = NED[1];
downPos = NED[2];
}
break;
default:
PIOS_Assert(0);
break;
}
northPos = NED[0];
eastPos = NED[1];
downPos = NED[2];
}
break;
default:
PIOS_Assert(0);
break;
}
// Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
float scale = 1;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
// Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
/**
@ -417,16 +538,16 @@ void updateEndpointVelocity()
*/
static void updateFixedAttitude(float* attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired);
}
/**
@ -436,128 +557,128 @@ static void updateFixedAttitude(float* attitude)
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
static void updateVtolDesiredAttitude(bool yaw_attitude)
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float northError;
float northCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
northVel = gpsVelocity.North;
eastVel = gpsVelocity.East;
downVel = gpsVelocity.Down;
}
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
float eastError;
float eastCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
northVel = gpsVelocity.North;
eastVel = gpsVelocity.East;
downVel = gpsVelocity.Down;
}
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
downVel = velocityActual.Down;
}
break;
default:
PIOS_Assert(0);
break;
}
// Testing code - refactor into manual control command
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
northVelIntegral -
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
eastVelIntegral -
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
downVelIntegral -
nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
downVel = velocityActual.Down;
}
break;
default:
PIOS_Assert(0);
break;
}
// Testing code - refactor into manual control command
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired);
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
}
StabilizationDesiredSet(&stabDesired);
}
/**
@ -565,39 +686,39 @@ static void updateVtolDesiredAttitude()
*/
static void updateNedAccel()
{
float accel[3];
float q[4];
float Rbe[3][3];
float accel_ned[3];
float accel[3];
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Collect downsampled attitude data
AccelsData accels;
AccelsGet(&accels);
accel[0] = accels.x;
accel[1] = accels.y;
accel[2] = accels.z;
//rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0]=attitudeActual.q1;
q[1]=attitudeActual.q2;
q[2]=attitudeActual.q3;
q[3]=attitudeActual.q4;
Quaternion2R(q, Rbe);
for (uint8_t i=0; i<3; i++){
accel_ned[i]=0;
for (uint8_t j=0; j<3; j++)
accel_ned[i] += Rbe[j][i]*accel[j];
}
accel_ned[2] += 9.81f;
NedAccelData accelData;
NedAccelGet(&accelData);
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);
// Collect downsampled attitude data
AccelsData accels;
AccelsGet(&accels);
accel[0] = accels.x;
accel[1] = accels.y;
accel[2] = accels.z;
//rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
Quaternion2R(q, Rbe);
for (uint8_t i = 0; i < 3; i++) {
accel_ned[i] = 0;
for (uint8_t j = 0; j < 3; j++)
accel_ned[i] += Rbe[j][i] * accel[j];
}
accel_ned[2] += 9.81f;
NedAccelData accelData;
NedAccelGet(&accelData);
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);
}
/**
@ -605,17 +726,41 @@ static void updateNedAccel()
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
}
static void accessoryUpdated(UAVObjEvent* ev)
{
if (ev->obj != AccessoryDesiredHandle())
return;
AccessoryDesiredData accessory;
PoiLearnSettingsData poiLearn;
PoiLearnSettingsGet(&poiLearn);
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
if (accessory.AccessoryVal < -0.5f) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
PoiLocationData poi;
PoiLocationGet(&poi);
poi.North = positionActual.North;
poi.East = positionActual.East;
poi.Down = positionActual.Down;
PoiLocationSet(&poi);
}
}
}
}

4
flight/pios/common/libraries/dosfs/dosfs.c
View File

@ -1194,7 +1194,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
// this point, all passes through the read loop will be aligned on a
// sector boundary, which allows us to go through the optimal path
// 2A below.
if (remain >= SECTOR_SIZE - tempsize) {
if (remain >= (uint32_t)SECTOR_SIZE - tempsize) {
memcpy(scratch + tempsize, buffer, SECTOR_SIZE - tempsize);
if (!result)
result = DFS_WriteSector(fileinfo->volinfo->unit, scratch, sector, 1);
@ -1330,7 +1330,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
No original function of DosFS driver
It has no effect if writing to SD Card, it's only used by the DosFS wrapper in emulation
*/
uint32_t DFS_Close(PFILEINFO fileinfo)
uint32_t DFS_Close(__attribute__((unused)) PFILEINFO fileinfo)
{
return DFS_OK;
}

2
flight/pios/common/libraries/msheap/msheap.c
View File

@ -423,7 +423,7 @@ msheap_extend(uint32_t size)
* @param reason The reason we are panicking.
*/
void
msheap_panic(const char *reason)
msheap_panic(__attribute__((unused)) const char *reason)
{
for (;;)
;

2
flight/pios/common/libraries/msheap/pios_msheap.c
View File

@ -134,7 +134,7 @@ free(void *p)
#endif /* PIOS_INCLUDE_FREERTOS */
void
msheap_panic(const char *reason)
msheap_panic(__attribute__((unused)) const char *reason)
{
//PIOS_DEBUG_Panic(reason);
}

2
flight/pios/common/pios_adxl345.c
View File

@ -187,7 +187,7 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth)
/**
* @brief Enable measuring. This also disables the activity sensors (tap or free fall)
*/
static int32_t PIOS_ADXL345_SetMeasure(uint8_t enable)
static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;

2
flight/pios/common/pios_bma180.c
View File

@ -438,7 +438,7 @@ bool PIOS_BMA180_IRQHandler(void)
{
bma180_irqs++;
const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t pios_bma180_dmabuf[8];
// If we can't get the bus then just move on for efficiency

4
flight/pios/common/pios_bmp085.c
View File

@ -311,8 +311,8 @@ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer)
int32_t PIOS_BMP085_Test()
{
// TODO: Is there a better way to test this than just checking that pressure/temperature has changed?
int32_t passed = 1;
int32_t cur_value = 0;
uint32_t passed = 1;
uint32_t cur_value = 0;
cur_value = Temperature;
PIOS_BMP085_StartADC(TemperatureConv);

4
flight/pios/common/pios_com_msg.c
View File

@ -76,7 +76,7 @@ int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driv
return(0);
}
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
{
struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;
@ -103,7 +103,7 @@ static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint
return (bytes_from_fifo);
}
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
{
struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;

11
flight/pios/common/pios_flash_jedec.c
View File

@ -303,12 +303,12 @@ static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
#else /* FLASH_USE_FREERTOS_LOCKS */
static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id)
static int32_t PIOS_Flash_Jedec_StartTransaction(__attribute__((unused)) uintptr_t flash_id)
{
return 0;
}
static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t flash_id)
{
return 0;
}
@ -387,12 +387,15 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id)
while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
#if defined(FLASH_FREERTOS)
vTaskDelay(1);
if ((i++) % 100 == 0)
if ((i++) % 100 == 0) {
#else
if ((i++) % 10000 == 0)
if ((i++) % 10000 == 0) {
#endif
#ifdef PIOS_LED_HEARTBEAT
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif
}
}

12
flight/pios/common/pios_flashfs_logfs.c
View File

@ -192,7 +192,9 @@ static int32_t logfs_erase_all_arenas()
uint16_t num_arenas = logfs.cfg->total_fs_size / logfs.cfg->arena_size;
for (uint16_t arena = 0; arena < num_arenas; arena++) {
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#ifdef PIOS_LED_HEARTBEAT
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif
if (logfs_erase_arena(arena) != 0)
return -1;
}
@ -815,7 +817,7 @@ static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_
* @retval -5 if filesystem is full even after garbage collection should have freed space
* @retval -6 if writing the new object to the filesystem failed
*/
int32_t PIOS_FLASHFS_ObjSave(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
int32_t PIOS_FLASHFS_ObjSave(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
{
int8_t rc;
@ -893,7 +895,7 @@ out_exit:
* @retval -3 if object size in filesystem does not exactly match buffer size
* @retval -4 if reading the object data from flash fails
*/
int32_t PIOS_FLASHFS_ObjLoad(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
int32_t PIOS_FLASHFS_ObjLoad(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
{
int8_t rc;
@ -952,7 +954,7 @@ out_exit:
* @retval -1 if failed to start transaction
* @retval -2 if failed to delete the object from the filesystem
*/
int32_t PIOS_FLASHFS_ObjDelete(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
int32_t PIOS_FLASHFS_ObjDelete(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
{
int8_t rc;
@ -985,7 +987,7 @@ out_exit:
* @retval -3 if failed to activate arena 0
* @retval -4 if failed to mount arena 0
*/
int32_t PIOS_FLASHFS_Format(uint32_t fs_id)
int32_t PIOS_FLASHFS_Format(__attribute__((unused)) uint32_t fs_id)
{
int32_t rc;

4
flight/pios/common/pios_gcsrcvr.c
View File

@ -113,7 +113,7 @@ static void gcsreceiver_updated(UAVObjEvent * ev)
}
}
extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id)
{
struct pios_gcsrcvr_dev *gcsrcvr_dev;
@ -145,7 +145,7 @@ extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
* \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
* \output >=0 channel value
*/
static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel)
{
if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
/* channel is out of range */

322
flight/pios/common/pios_hcsr04.c
View File

@ -29,6 +29,7 @@
*/
#include "pios.h"
#include "pios_hcsr04_priv.h"
#ifdef PIOS_INCLUDE_HCSR04
@ -37,173 +38,260 @@
#endif
/* Local Variables */
/* 100 ms timeout without updates on channels */
const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
static TIM_ICInitTypeDef TIM_ICInitStructure;
static uint8_t CaptureState;
static uint16_t RiseValue;
static uint16_t FallValue;
static uint32_t CaptureValue;
static uint32_t CapCounter;
struct pios_hcsr04_dev * hcsr04_dev_loc;
enum pios_hcsr04_dev_magic {
PIOS_HCSR04_DEV_MAGIC = 0xab3029AA,
};
struct pios_hcsr04_dev {
enum pios_hcsr04_dev_magic magic;
const struct pios_hcsr04_cfg * cfg;
uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
uint32_t us_since_update[PIOS_PWM_NUM_INPUTS];
};
static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev * hcsr04_dev)
{
return (hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_hcsr04_dev * PIOS_PWM_alloc(void)
{
struct pios_hcsr04_dev * hcsr04_dev;
hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev));
if (!hcsr04_dev) return(NULL);
hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
return(hcsr04_dev);
}
#else
static struct pios_hcsr04_dev pios_hcsr04_devs[PIOS_PWM_MAX_DEVS];
static uint8_t pios_hcsr04_num_devs;
static struct pios_hcsr04_dev * PIOS_PWM_alloc(void)
{
struct pios_hcsr04_dev * hcsr04_dev;
if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) {
return (NULL);
}
hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++];
hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
return (hcsr04_dev);
}
#endif
static void PIOS_HCSR04_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_HCSR04_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_HCSR04_tim_overflow_cb,
.edge = PIOS_HCSR04_tim_edge_cb,
};
#define PIOS_HCSR04_TRIG_GPIO_PORT GPIOD
#define PIOS_HCSR04_TRIG_PIN GPIO_Pin_2
/**
* Initialise the HC-SR04 sensor
* Initialises all the pins
*/
void PIOS_HCSR04_Init(void)
int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg)
{
/* Init triggerpin */
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = PIOS_HCSR04_TRIG_PIN;
GPIO_Init(PIOS_HCSR04_TRIG_GPIO_PORT, &GPIO_InitStructure);
PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN;
PIOS_DEBUG_Assert(pwm_id);
PIOS_DEBUG_Assert(cfg);
/* Flush counter variables */
CaptureState = 0;
RiseValue = 0;
FallValue = 0;
CaptureValue = 0;
struct pios_hcsr04_dev * hcsr04_dev;
/* Setup RCC */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
hcsr04_dev = (struct pios_hcsr04_dev *) PIOS_PWM_alloc();
if (!hcsr04_dev) goto out_fail;
/* Enable timer interrupts */
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_Init(&NVIC_InitStructure);
/* Bind the configuration to the device instance */
hcsr04_dev->cfg = cfg;
hcsr04_dev_loc = hcsr04_dev;
/* Partial pin remap for TIM3 (PB5) */
GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
/* Flush counter variables */
hcsr04_dev->CaptureState[i] = 0;
hcsr04_dev->RiseValue[i] = 0;
hcsr04_dev->FallValue[i] = 0;
hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
}
/* Configure input pins */
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_Init(GPIOB, &GPIO_InitStructure);
uint32_t tim_id;
if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) {
return -1;
}
/* Configure timer for input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
/* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer clocks */
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 500000) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_InternalClockConfig(TIM3);
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
/* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
/* Enable the Capture Compare Interrupt Request */
//TIM_ITConfig(PIOS_PWM_CH8_TIM_PORT, PIOS_PWM_CH8_CCR, ENABLE);
TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE);
/* Enable the Capture Compare Interrupt Request */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE);
break;
case TIM_Channel_2:
TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE);
break;
case TIM_Channel_3:
TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE);
break;
case TIM_Channel_4:
TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE);
break;
}
/* Enable timers */
TIM_Cmd(TIM3, ENABLE);
// Need the update event for that timer to detect timeouts
TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE);
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
}
#ifndef STM32F4XX
/* Enable the peripheral clock for the GPIO */
switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) {
case (uint32_t) GPIOA:
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
break;
case (uint32_t) GPIOB:
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
break;
case (uint32_t) GPIOC:
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
break;
default:
PIOS_Assert(0);
break;
}
#endif
GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init);
*pwm_id = (uint32_t) hcsr04_dev;
return (0);
out_fail:
return (-1);
}
void PIOS_HCSR04_Trigger(void)
{
GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
PIOS_DELAY_WaituS(15);
GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
}
/**
* Get the value of an sonar timer
* \output >0 timer value
* Get the value of an input channel
* \param[in] Channel Number of the channel desired
* \output -1 Channel not available
* \output >0 Channel value
*/
int32_t PIOS_HCSR04_Get(void)
{
return CaptureValue;
return hcsr04_dev_loc->CaptureValue[0];
}
/**
* Get the value of an sonar timer
* \output >0 timer value
*/
int32_t PIOS_HCSR04_Completed(void)
{
return CapCounter;
}
/**
* Trigger sonar sensor
*/
void PIOS_HCSR04_Trigger(void)
{
CapCounter=0;
PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN;
PIOS_DELAY_WaituS(15);
PIOS_HCSR04_TRIG_GPIO_PORT->BRR = PIOS_HCSR04_TRIG_PIN;
TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
return hcsr04_dev_loc->CapCounter[0];
}
/**
* Handle TIM3 global interrupt request
*/
//void PIOS_PWM_irq_handler(TIM_TypeDef * timer)
void TIM3_IRQHandler(void)
static void PIOS_HCSR04_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
{
/* Zero value always will be changed but this prevents compiler warning */
int32_t i = 0;
struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context;
/* Do this as it's more efficient */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET) {
i = 7;
if (CaptureState == 0) {
RiseValue = TIM_GetCapture2(TIM3);
} else {
FallValue = TIM_GetCapture2(TIM3);
}
if (!PIOS_HCSR04_validate(hcsr04_dev)) {
/* Invalid device specified */
return;
}
/* Clear TIM3 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
if (channel >= hcsr04_dev->cfg->num_channels) {
/* Channel out of range */
return;
}
hcsr04_dev->us_since_update[channel] += count;
if(hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
hcsr04_dev->CaptureState[channel] = 0;
hcsr04_dev->RiseValue[channel] = 0;
hcsr04_dev->FallValue[channel] = 0;
hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT;
hcsr04_dev->us_since_update[channel] = 0;
}
return;
}
static void PIOS_HCSR04_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context;
if (!PIOS_HCSR04_validate(hcsr04_dev)) {
/* Invalid device specified */
return;
}
if (chan_idx >= hcsr04_dev->cfg->num_channels) {
/* Channel out of range */
return;
}
const struct pios_tim_channel * chan = &hcsr04_dev->cfg->channels[chan_idx];
if (hcsr04_dev->CaptureState[chan_idx] == 0) {
hcsr04_dev->RiseValue[chan_idx] = count;
hcsr04_dev->us_since_update[chan_idx] = 0;
} else {
hcsr04_dev->FallValue[chan_idx] = count;
}
// flip state machine and capture value here
/* Simple rise or fall state machine */
if (CaptureState == 0) {
TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init;
if (hcsr04_dev->CaptureState[chan_idx] == 0) {
/* Switch states */
CaptureState = 1;
hcsr04_dev->CaptureState[chan_idx] = 1;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
} else {
/* Capture computation */
if (FallValue > RiseValue) {
CaptureValue = (FallValue - RiseValue);
if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) {
hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]);
} else {
CaptureValue = ((0xFFFF - RiseValue) + FallValue);
hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]);
}
/* Switch states */
CaptureState = 0;
hcsr04_dev->CaptureState[chan_idx] = 0;
/* Increase supervisor counter */
CapCounter++;
TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE);
hcsr04_dev->CapCounter[chan_idx]++;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
}
}
#endif /* PIOS_INCLUDE_HCSR04 */

4
flight/pios/common/pios_mpu6000.c
View File

@ -454,7 +454,7 @@ bool PIOS_MPU6000_IRQHandler(void)
return false;
mpu6000_count = PIOS_MPU6000_FifoDepth(true);
if (mpu6000_count < sizeof(struct pios_mpu6000_data))
if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data))
return false;
if (PIOS_MPU6000_ClaimBus(true) != 0)
@ -474,7 +474,7 @@ bool PIOS_MPU6000_IRQHandler(void)
static struct pios_mpu6000_data data;
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (sizeof(data) * 2)) {
if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;

1977
flight/pios/common/pios_rfm22b.c
View File

@ -71,7 +71,6 @@
#define RFM22B_MAXIMUM_FREQUENCY 440000000
#define RFM22B_DEFAULT_FREQUENCY 433000000
#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000
//#define RFM22B_TEST_DROPPED_PACKETS 1
// The maximum amount of time since the last message received to consider the connection broken.
#define DISCONNECT_TIMEOUT_MS 1000 // ms
@ -125,8 +124,8 @@
/* Local type definitions */
struct pios_rfm22b_transition {
enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
enum pios_rfm22b_state next_state[RFM22B_EVENT_NUM_EVENTS];
enum pios_radio_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS];
};
// Must ensure these prefilled arrays match the define sizes
@ -157,42 +156,39 @@ static const uint8_t OUT_FF[64] = {
/* Local function forwared declarations */
static void pios_rfm22_task(void *parameters);
static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR);
static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR);
static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status);
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size);
static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel);
static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev);
static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks);
static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev);
static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_clearLEDs();
@ -206,220 +202,187 @@ static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
/* The state transition table */
const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = {
static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
// Initialization thread
[RFM22B_STATE_UNINITIALIZED] = {
[RADIO_STATE_UNINITIALIZED] = {
.entry_fn = 0,
.next_state = {
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
},
},
[RFM22B_STATE_INITIALIZING] = {
[RADIO_STATE_INITIALIZING] = {
.entry_fn = rfm22_init,
.next_state = {
[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_REQUESTING_CONNECTION] = {
[RADIO_STATE_REQUESTING_CONNECTION] = {
.entry_fn = rfm22_requestConnection,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR
},
},
[RFM22B_STATE_ACCEPTING_CONNECTION] = {
[RADIO_STATE_ACCEPTING_CONNECTION] = {
.entry_fn = rfm22_acceptConnection,
.next_state = {
[RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_MODE] = {
.entry_fn = rfm22_setRxMode,
[RADIO_STATE_RX_MODE] = {
.entry_fn = radio_setRxMode,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
[RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_WAIT_PREAMBLE] = {
.entry_fn = rfm22_detectPreamble,
[RADIO_STATE_RX_DATA] = {
.entry_fn = radio_rxData,
.next_state = {
[RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC,
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION,
[RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS,
[RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION,
[RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK,
[RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_WAIT_SYNC] = {
.entry_fn = rfm22_detectSync,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC,
[RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_DATA] = {
.entry_fn = rfm22_rxData,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA,
[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK,
[RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS,
[RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION,
[RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK,
[RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_FAILURE] = {
.entry_fn = rfm22_rxFailure,
.next_state = {
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_ACK] = {
[RADIO_STATE_RECEIVING_ACK] = {
.entry_fn = rfm22_receiveAck,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_NACK] = {
[RADIO_STATE_RECEIVING_NACK] = {
.entry_fn = rfm22_receiveNack,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_STATUS] = {
[RADIO_STATE_RECEIVING_STATUS] = {
.entry_fn = rfm22_receiveStatus,
.next_state = {
[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_START] = {
.entry_fn = rfm22_txStart,
[RADIO_STATE_TX_START] = {
.entry_fn = radio_txStart,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_DATA] = {
.entry_fn = rfm22_txData,
[RADIO_STATE_TX_DATA] = {
.entry_fn = radio_txData,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
[RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_FAILURE] = {
[RADIO_STATE_TX_FAILURE] = {
.entry_fn = rfm22_txFailure,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_SENDING_ACK] = {
[RADIO_STATE_SENDING_ACK] = {
.entry_fn = rfm22_sendAck,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_SENDING_NACK] = {
[RADIO_STATE_SENDING_NACK] = {
.entry_fn = rfm22_sendNack,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TIMEOUT] = {
[RADIO_STATE_TIMEOUT] = {
.entry_fn = rfm22_timeout,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_ERROR] = {
[RADIO_STATE_ERROR] = {
.entry_fn = rfm22_error,
.next_state = {
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_FATAL_ERROR] = {
[RADIO_STATE_FATAL_ERROR] = {
.entry_fn = rfm22_fatal_error,
.next_state = {
},
@ -511,6 +474,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->stats.rssi = 0;
rfm22b_dev->stats.tx_seq = 0;
rfm22b_dev->stats.rx_seq = 0;
rfm22b_dev->stats.tx_failure = 0;
// Initialize the frequencies.
PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY);
@ -523,7 +487,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->coordinator = false;
// Create the event queue
rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event));
rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
// Bind the configuration to the device instance
rfm22b_dev->cfg = *cfg;
@ -560,10 +524,10 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
initialize_ecc();
// Set the state to initializing.
rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED;
rfm22b_dev->state = RADIO_STATE_UNINITIALIZED;
// Initialize the radio device.
pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
// Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler.
xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle));
@ -580,7 +544,7 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (PIOS_RFM22B_Validate(rfm22b_dev)) {
pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
}
}
@ -594,7 +558,7 @@ bool PIOS_RFM22_EXT_Int(void)
}
// Inject an interrupt event into the state machine.
pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true);
pios_rfm22_inject_event(g_rfm22b_dev, RADIO_EVENT_INT_RECEIVED, true);
return false;
}
@ -628,6 +592,21 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id)
return false;
}
/**
* Returns true if the modem is not actively sending or receiving a packet.
*
* @param[in] rfm22b_id The RFM22B device index.
* @return True if the modem is not actively sending or receiving a packet.
*/
bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (PIOS_RFM22B_Validate(rfm22b_dev)) {
return ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION));
}
return false;
}
/**
* Sets the radio device transmit power.
*
@ -790,6 +769,350 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id)
return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD));
}
/**
* Put the RFM22B device into receive mode.
*
* @param[in] rfm22b_id The rfm22b device.
* @param[in] p The packet to receive into.
* @return true if Rx mode was entered sucessfully.
*/
bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return false;
}
// Are we already in Rx mode?
if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT)) {
return false;
}
rfm22b_dev->rx_packet_handle = p;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// Switch to TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_OFF;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
RX_LED_OFF;
TX_LED_OFF;
// empty the rx buffer
rfm22b_dev->rx_buffer_wr = 0;
// Clear the TX buffer.
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// enable RX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
RFM22_ie2_enswdet);
// enable the receiver
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX wait mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT;
return true;
}
/**
* Transmit a packet via the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @param[in] p The packet to transmit.
* @return true if there if the packet was queued for transmission.
*/
bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return false;
}
rfm22b_dev->tx_packet = p;
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0) {
rfm22b_dev->packet_start_ticks = 1;
}
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// set the tx power
rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
// TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
// Queue the data up for sending
rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
RX_LED_OFF;
// Set the destination address in the transmit header.
// The destination address is the first 4 bytes of the message.
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk);
// Clear the FIFOs.
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// Set the total number of data bytes we are going to transmit.
rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
// Add some data to the chips TX FIFO before enabling the transmitter
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
// Enable TX interrupts.
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// Enable the transmitter.
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// We're in Tx mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_TX_MODE;
TX_LED_ON;
return true;
}
/**
* Process a Tx interrupt from the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
*/
pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
// Read the device status registers
if (!pios_rfm22_readStatus(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
// TX FIFO almost empty, it needs filling up
if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) {
// Add data to the TX FIFO buffer
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
return PIOS_RFM22B_INT_SUCCESS;
} else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) {
// Transition out of Tx mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
return PIOS_RFM22B_TX_COMPLETE;
}
return 0;
}
/**
* Process a Rx interrupt from the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
*/
pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle);
pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS;
// Read the device status registers
if (!pios_rfm22_readStatus(rfm22b_dev)) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// FIFO under/over flow error. Restart RX mode.
if (rfm22b_dev->status_regs.int_status_1.fifo_underoverflow_error ||
rfm22b_dev->status_regs.int_status_1.crc_error) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Valid packet received
if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
}
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Is there a length error?
if (rfm22b_dev->rx_buffer_wr != len) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Increment the total byte received count.
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
// We're finished with Rx mode
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
ret = PIOS_RFM22B_RX_COMPLETE;
} else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
// RX FIFO almost full, it needs emptying
// read data from the rf chips FIFO buffer
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
rfm22_releaseBus(rfm22b_dev);
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs(rfm22b_dev);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Make sure that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Valid preamble detected
RX_LED_ON;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// We detected the preamble, now wait for sync.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
} else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
// Sync word detected
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Waiting for the preamble timed out.
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Set the packet start time if necessary.
if ((rfm22b_dev->packet_start_ticks == 0) &&
((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) {
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
}
return ret;
}
/**
* Validate that the device structure is valid.
*
@ -831,10 +1154,10 @@ static void pios_rfm22_task(void *parameters)
lastEventTicks = xTaskGetTickCount();
// Process events through the state machine.
enum pios_rfm22b_event event;
enum pios_radio_event event;
while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
if ((event == RFM22B_EVENT_INT_RECEIVED) &&
((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING)))
if ((event == RADIO_EVENT_INT_RECEIVED) &&
((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING)))
continue;
rfm22_process_event(rfm22b_dev, event);
}
@ -843,10 +1166,10 @@ static void pios_rfm22_task(void *parameters)
portTickType curTicks = xTaskGetTickCount();
if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) {
// Transsition through an error event.
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
// Clear the event queue.
enum pios_rfm22b_event event;
enum pios_radio_event event;
while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
// Do nothing;
}
@ -855,28 +1178,29 @@ static void pios_rfm22_task(void *parameters)
}
// Change channels if necessary.
if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) {
if (PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22_changeChannel(rfm22b_dev);
}
portTickType curTicks = xTaskGetTickCount();
uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks);
// Have we been sending this packet too long?
if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT);
// Has it been too long since we received a packet
// Have we been sending / receiving this packet too long?
if ((rfm22b_dev->packet_start_ticks > 0) &&
(pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT);
} else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
// Has it been too long since we received a packet
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
} else {
// Are we waiting for an ACK?
if (rfm22b_dev->prev_tx_packet) {
// Should we resend the packet?
if (pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) {
if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) &&
PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22b_dev->tx_complete_ticks = curTicks;
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT);
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT);
}
} else {
@ -888,7 +1212,7 @@ static void pios_rfm22_task(void *parameters)
}
// Queue up a status packet if it's time.
if ((pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) {
if (pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) {
rfm22_sendStatus(rfm22b_dev);
lastStatusTicks = curTicks;
}
@ -897,21 +1221,16 @@ static void pios_rfm22_task(void *parameters)
}
// Send a packet if it's our time slice
rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0);
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
if (rfm22b_dev->time_to_send) {
bool time_to_send = rfm22_timeToSend(rfm22b_dev);
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
if (time_to_send) {
D4_LED_ON;
} else {
D4_LED_OFF;
}
if (rfm22_inChannelBuffer(rfm22b_dev)) {
D3_LED_ON;
} else {
D3_LED_OFF;
}
#endif
if (rfm22b_dev->time_to_send) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START);
if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START);
}
}
}
@ -928,22 +1247,25 @@ static void pios_rfm22_task(void *parameters)
* @param[in] event The event to inject
* @param[in] inISR Is this being called from an interrrup service routine?
*/
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR)
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR)
{
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (inISR) {
portBASE_TYPE pxHigherPriorityTaskWoken;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) {
// Store the event.
portBASE_TYPE pxHigherPriorityTaskWoken1;
if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
portBASE_TYPE pxHigherPriorityTaskWoken2;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
}
portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken);
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
} else {
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
@ -956,20 +1278,20 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
*
* @param[in] rfm22b_dev The device structure
* @param[in] event The event to process
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
{
// No event
if (event == RFM22B_EVENT_NUM_EVENTS) {
return RFM22B_EVENT_NUM_EVENTS;
if (event >= RADIO_EVENT_NUM_EVENTS) {
return RADIO_EVENT_NUM_EVENTS;
}
// Don't transition if there is no transition defined
enum pios_rfm22b_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
enum pios_radio_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
if (!next_state) {
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/*
@ -986,7 +1308,7 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev);
}
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/**
@ -996,10 +1318,10 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
* @param[in] rfm22b_dev The device structure
* @param[in] event The event to process
*/
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
{
// Process all state transitions.
while(event != RFM22B_EVENT_NUM_EVENTS) {
while(event != RADIO_EVENT_NUM_EVENTS) {
event = rfm22_process_state_transition(rfm22b_dev, event);
}
}
@ -1013,16 +1335,16 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf
* Initialize (or re-initialize) the RFM22B radio device.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
{
// Initialize the register values.
rfm22b_dev->device_status = 0;
rfm22b_dev->int_status1 = 0;
rfm22b_dev->int_status2 = 0;
rfm22b_dev->ezmac_status = 0;
rfm22b_dev->status_regs.int_status_1.raw = 0;
rfm22b_dev->status_regs.int_status_2.raw = 0;
rfm22b_dev->status_regs.device_status.raw = 0;
rfm22b_dev->status_regs.ezmac_status.raw = 0;
// Clean the LEDs
rfm22_clearLEDs();
@ -1043,8 +1365,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// Initialize the state
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
rfm22b_dev->destination_id = 0xffffffff;
rfm22b_dev->time_to_send = false;
rfm22b_dev->time_to_send_offset = 0;
rfm22b_dev->send_status = false;
rfm22b_dev->send_connection_request = false;
@ -1053,10 +1373,8 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->tx_packet = NULL;
rfm22b_dev->prev_tx_packet = NULL;
rfm22b_dev->data_packet.header.data_size = 0;
rfm22b_dev->in_rx_mode = false;
// Initialize the devide state
rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
rfm22b_dev->frequency_hop_channel = 0;
@ -1064,27 +1382,31 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_complete_ticks = 0;
rfm22b_dev->rx_complete_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING;
// software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
for (uint8_t i = 0; i < 50; ++i) {
for (int i = 50; i > 0; i--) {
// read the status registers
rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break;
pios_rfm22_readStatus(rfm22b_dev);
// wait 1ms
// Is the chip ready?
if (rfm22b_dev->status_regs.int_status_2.chip_ready)
break;
// Wait 1ms if not.
PIOS_DELAY_WaitmS(1);
}
// ****************
// read status - clears interrupt
rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status);
rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
pios_rfm22_readStatus(rfm22b_dev);
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// disable all interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
@ -1108,14 +1430,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
#endif
// incorrect RF module type
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
if (device_version != RFM22_DEVICE_VERSION_B1) {
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r");
#endif
// incorrect RF module version
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
// calibrate our RF module to be exactly on frequency .. different for every module
@ -1182,14 +1504,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// header control - using a 4 by header with broadcast of 0xffffffff
rfm22_write(rfm22b_dev, RFM22_header_control1,
RFM22_header_cntl1_bcen_0 |
RFM22_header_cntl1_bcen_1 |
RFM22_header_cntl1_bcen_2 |
RFM22_header_cntl1_bcen_3 |
RFM22_header_cntl1_hdch_0 |
RFM22_header_cntl1_hdch_1 |
RFM22_header_cntl1_hdch_2 |
RFM22_header_cntl1_hdch_3);
RFM22_header_cntl1_bcen_0 |
RFM22_header_cntl1_bcen_1 |
RFM22_header_cntl1_bcen_2 |
RFM22_header_cntl1_bcen_3 |
RFM22_header_cntl1_hdch_0 |
RFM22_header_cntl1_hdch_1 |
RFM22_header_cntl1_hdch_2 |
RFM22_header_cntl1_hdch_3);
// Check all bit of all bytes of the header
rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff);
rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff);
@ -1203,9 +1525,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff);
// 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
rfm22_write(rfm22b_dev, RFM22_header_control2,
RFM22_header_cntl2_hdlen_3210 |
RFM22_header_cntl2_synclen_3210 |
((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
RFM22_header_cntl2_hdlen_3210 |
RFM22_header_cntl2_synclen_3210 |
((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
// sync word
rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1);
@ -1213,9 +1535,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3);
rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4);
// set the tx power
rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
// TX FIFO Almost Full Threshold (0 - 63)
rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK);
@ -1228,11 +1547,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// Set the frequency calibration
rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// Initialize the frequency and datarate to te default.
rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE);
rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
pios_rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
return RFM22B_EVENT_INITIALIZED;
return RADIO_EVENT_INITIALIZED;
}
/**
@ -1248,7 +1570,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
* @param[in] datarate The air datarate.
* @param[in] data_whitening Is data whitening desired?
*/
static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
{
uint32_t datarate_bps = data_rate[datarate];
rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
@ -1257,6 +1579,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_da
uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03;
rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// rfm22_if_filter_bandwidth
rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
@ -1309,6 +1634,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_da
rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00);
rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
}
/**
@ -1338,6 +1666,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
uint8_t fch = (fc >> 8) & 0xff;
uint8_t fcl = fc & 0xff;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Calculate the number of frequency hopping channels.
rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size);
@ -1363,6 +1694,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff);
rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch);
rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
}
@ -1377,22 +1711,20 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t
if (rfm22b_dev->frequency_hop_channel == channel) {
return false;
}
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D3_LED_TOGGLE;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
rfm22b_dev->frequency_hop_channel = channel;
rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
return true;
}
/*****************************************************************************
* Radio Transmit and Receive functions.
*****************************************************************************/
/**
* Read the RFM22B interrupt and device status registers
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
// 1. Read the interrupt statuses with burst read
@ -1402,327 +1734,217 @@ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL);
rfm22_deassertCs(rfm22b_dev);
rfm22b_dev->int_status1 = read_buf[1];
rfm22b_dev->int_status2 = read_buf[2];
rfm22b_dev->status_regs.int_status_1.raw = read_buf[1];
rfm22b_dev->status_regs.int_status_2.raw = read_buf[2];
// Device status
rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status);
rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status);
// EzMAC status
rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status);
rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// the RF module has gone and done a reset - we need to re-initialize the rf module
if (rfm22b_dev->int_status2 & RFM22_is2_ipor) {
if (rfm22b_dev->status_regs.int_status_2.poweron_reset) {
return false;
}
return true;
}
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
}
/*****************************************************************************
* Radio Transmit and Receive functions.
*****************************************************************************/
/**
* Start a transmit if possible
*
* @param[in] radio_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
{
PHPacketHandle p = NULL;
// Don't send if it's not our turn, or if we're receiving a packet.
if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) {
return RADIO_EVENT_RX_MODE;
}
// See if there's a packet ready to send.
if (radio_dev->tx_packet) {
p = radio_dev->tx_packet;
} else {
// Don't send a packet if we're waiting for an ACK
if (radio_dev->prev_tx_packet) {
return RADIO_EVENT_RX_MODE;
}
// Send a connection request?
if (!p && radio_dev->send_connection_request) {
p = (PHPacketHandle)&(radio_dev->con_packet);
radio_dev->send_connection_request = false;
}
#ifdef PIOS_PPM_RECEIVER
// Send a PPM packet?
if (!p && radio_dev->send_ppm) {
p = (PHPacketHandle)&(radio_dev->ppm_packet);
radio_dev->send_ppm = false;
}
#endif
// Send status?
if (!p && radio_dev->send_status) {
p = (PHPacketHandle)&(radio_dev->status_packet);
radio_dev->send_status = false;
}
// Try to get some data to send
if (!p) {
bool need_yield = false;
p = &radio_dev->data_packet;
p->header.type = PACKET_TYPE_DATA;
p->header.destination_id = radio_dev->destination_id;
if (radio_dev->tx_out_cb && (p->header.data_size == 0)) {
p->header.data_size = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
}
// Don't send any data until we're connected.
if (!rfm22_isConnected(radio_dev)) {
p->header.data_size = 0;
}
if (p->header.data_size == 0) {
p = NULL;
}
}
}
if (!p) {
return RADIO_EVENT_RX_MODE;
}
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_ON;
#endif
// Add the packet sequence number.
p->header.seq_num = radio_dev->stats.tx_seq++;
// Pass the time of the previous transmitted packet to use for synchronizing the clocks.
p->header.prev_tx_time = radio_dev->tx_complete_ticks;
// Change the channel if necessary, but not when ACKing the connection request message.
if ((p->header.type != PACKET_TYPE_ACK) || (radio_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
rfm22_changeChannel(radio_dev);
}
// Add the error correcting code.
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
// Transmit the packet.
PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p);
return RADIO_EVENT_NUM_EVENTS;
}
/**
* Receive packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev)
{
enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
pios_rfm22b_int_result res = PIOS_RFM22B_ProcessTx((uint32_t)radio_dev);
// Is the transmition complete
if (res == PIOS_RFM22B_TX_COMPLETE) {
radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet);
radio_dev->tx_complete_ticks = xTaskGetTickCount();
// Is this an ACK?
bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK);
ret_event = RADIO_EVENT_RX_MODE;
if (is_ack) {
// If this is an ACK for a connection request message we need to
// configure this modem from the connection request message.
if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22_setConnectionParameters(radio_dev);
}
} else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) &&
(radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM)) {
// We need to wait for an ACK if this packet it not an ACK, NACK, or PPM.
radio_dev->prev_tx_packet = radio_dev->tx_packet;
}
radio_dev->tx_packet = 0;
radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0;
// Start a new transaction
radio_dev->packet_start_ticks = 0;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
#endif
}
return ret_event;
}
/**
* Switch the radio into receive mode.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
{
// Are we already in Rx mode?
if (rfm22b_dev->in_rx_mode) {
return RFM22B_EVENT_NUM_EVENTS;
if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, &(rfm22b_dev->rx_packet))) {
return RADIO_EVENT_NUM_EVENTS;
}
rfm22b_dev->packet_start_ticks = 0;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// Switch to TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
RX_LED_OFF;
TX_LED_OFF;
// empty the rx buffer
rfm22b_dev->rx_buffer_wr = 0;
// Clear the TX buffer.
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// enable RX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
RFM22_ie2_enswdet);
// enable the receiver
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
// Indicate that we're in RX mode.
rfm22b_dev->in_rx_mode = true;
// No event generated
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Detect the preamble
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev)
{
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev))
return RFM22B_EVENT_FAILURE;
// Valid preamble detected
if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) {
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
RX_LED_ON;
return RFM22B_EVENT_PREAMBLE_DETECTED;
}
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Detect the sync
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev)
{
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev))
return RFM22B_EVENT_FAILURE;
// Sync word detected
if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) {
RX_LED_ON;
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
return RFM22B_EVENT_SYNC_DETECTED;
} else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) {
// Waiting for sync timed out.
return RFM22B_EVENT_FAILURE;
}
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Receive the packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
{
// Swap in the next packet buffer if required.
uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet);
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev)) {
return RFM22B_EVENT_FAILURE;
}
// FIFO under/over flow error. Restart RX mode.
if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) {
return RFM22B_EVENT_FAILURE;
}
// RX FIFO almost full, it needs emptying
if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) {
// read data from the rf chips FIFO buffer
// read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
return RFM22B_EVENT_FAILURE;
}
// Another packet length error.
if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) {
return RFM22B_EVENT_FAILURE;
}
// Fetch the data from the RX FIFO
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
}
// CRC error .. discard the received data
if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) {
return RFM22B_EVENT_FAILURE;
}
// Valid packet received
if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) {
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
}
if (rfm22b_dev->rx_buffer_wr != len) {
return RFM22B_EVENT_FAILURE;
}
// we have a valid received packet
enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE;
if (rfm22b_dev->rx_buffer_wr > 0) {
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
// Check the packet for errors.
if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) {
switch (rfm22b_dev->rx_packet.header.type) {
case PACKET_TYPE_STATUS:
ret_event = RFM22B_EVENT_STATUS_RECEIVED;
break;
case PACKET_TYPE_CON_REQUEST:
ret_event = RFM22B_EVENT_CONNECTION_REQUESTED;
break;
case PACKET_TYPE_DATA:
{
// Send the data to the com port
bool rx_need_yield;
if (rfm22b_dev->rx_in_cb)
(rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield);
#ifdef RFM22B_TEST_DROPPED_PACKETS
// Inject radnom missed ACKs
{
static uint8_t crc = 0;
static uint8_t cntr = 0;
crc = PIOS_CRC_updateByte(crc, cntr++);
if ((crc & 0x7) == 0)
ret_event = RFM22B_EVENT_RX_MODE;
}
#endif
break;
}
case PACKET_TYPE_DUPLICATE_DATA:
break;
case PACKET_TYPE_ACK:
case PACKET_TYPE_ACK_RTS:
ret_event = RFM22B_EVENT_PACKET_ACKED;
break;
case PACKET_TYPE_NACK:
ret_event = RFM22B_EVENT_PACKET_NACKED;
break;
case PACKET_TYPE_PPM:
{
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet);
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
bool ppm_output = false;
#endif
#endif
#if defined(PIOS_INCLUDE_RFM22B_RCVR)
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
rfm22b_dev->ppm_channel[i] = ppmp->channels[i];
}
#endif
#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
if (PIOS_PPM_OUTPUT) {
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
}
}
#endif
#if defined(PIOS_INCLUDE_GCSRCVR)
if (!ppm_output) {
GCSReceiverData gcsRcvr;
for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
gcsRcvr.Channel[i] = ppmp->channels[i];
}
GCSReceiverSet(&gcsRcvr);
}
#endif
break;
}
default:
break;
}
}
else {
ret_event = RFM22B_EVENT_RX_ERROR;
}
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
if (rfm22b_dev->rx_complete_ticks == 0)
rfm22b_dev->rx_complete_ticks = 1;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D2_LED_OFF;
#endif
}
// We're finished with Rx mode
rfm22b_dev->in_rx_mode = false;
// Start a new transaction
rfm22b_dev->packet_start_ticks = 0;
return ret_event;
}
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/**
* Complete the receipt of a packet.
*
* @param[in] rfm22b_dev The device structure
* @param[in] radio_dev The device structure
* @param[in] p The packet handle of the received packet.
* @param[in] rc_len The number of bytes received.
* @return enum pios_radio_event The next event to inject
*/
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len)
static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, PHPacketHandle p, uint16_t rx_len)
{
portTickType curTicks = xTaskGetTickCount();
// Attempt to correct any errors in the packet.
decode_data((unsigned char*)p, rx_len);
@ -1735,285 +1957,167 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand
corrected_packet = true;
}
// Add any missed packets into the stats.
bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK));
if (!ack_nack_packet && (good_packet || corrected_packet)) {
uint16_t seq_num = p->header.seq_num;
if (rfm22_isConnected(rfm22b_dev)) {
static bool first_time = true;
uint16_t missed_packets = 0;
if (first_time) {
first_time = false;
} else {
uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq;
if (seq_num > prev_seq_num)
missed_packets = seq_num - prev_seq_num - 1;
else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA))
p->header.type = PACKET_TYPE_DUPLICATE_DATA;
}
rfm22b_dev->stats.rx_missed += missed_packets;
}
rfm22b_dev->stats.rx_seq = seq_num;
}
// Set the packet status
if (good_packet) {
rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET);
} else if(corrected_packet) {
// We corrected the error.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET);
} else {
// We couldn't correct the error, so drop the packet.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_ERROR_RX_PACKET);
}
return (good_packet || corrected_packet);
}
enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE;
if (good_packet || corrected_packet) {
switch (p->header.type) {
case PACKET_TYPE_STATUS:
ret_event = RADIO_EVENT_STATUS_RECEIVED;
/**
* Start a transmit if possible
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
{
PHPacketHandle p = NULL;
// Don't send if it's not our turn.
if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) {
return RFM22B_EVENT_RX_MODE;
}
// See if there's a packet ready to send.
if (rfm22b_dev->tx_packet) {
p = rfm22b_dev->tx_packet;
} else {
// Don't send a packet if we're waiting for an ACK
if (rfm22b_dev->prev_tx_packet) {
return RFM22B_EVENT_RX_MODE;
}
// Send a connection request?
if (!p && rfm22b_dev->send_connection_request) {
p = (PHPacketHandle)&(rfm22b_dev->con_packet);
rfm22b_dev->send_connection_request = false;
}
#ifdef PIOS_PPM_RECEIVER
// Send a PPM packet?
if (!p && rfm22b_dev->send_ppm) {
p = (PHPacketHandle)&(rfm22b_dev->ppm_packet);
rfm22b_dev->send_ppm = false;
}
#endif
// Send status?
if (!p && rfm22b_dev->send_status) {
p = (PHPacketHandle)&(rfm22b_dev->status_packet);
rfm22b_dev->send_status = false;
}
// Try to get some data to send
if (!p) {
bool need_yield = false;
p = &rfm22b_dev->data_packet;
p->header.type = PACKET_TYPE_DATA;
p->header.destination_id = rfm22b_dev->destination_id;
if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) {
p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
}
// Don't send any data until we're connected.
if (!rfm22_isConnected(rfm22b_dev)) {
p->header.data_size = 0;
}
if (p->header.data_size == 0) {
p = NULL;
}
}
if (p) {
p->header.seq_num = rfm22b_dev->stats.tx_seq++;
}
}
if (!p) {
return RFM22B_EVENT_RX_MODE;
}
// We're transitioning out of Rx mode.
rfm22b_dev->in_rx_mode = false;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D1_LED_ON;
D2_LED_OFF;
#endif
// Change the channel if necessary.
if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) ||
(rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
rfm22_changeChannel(rfm22b_dev);
}
// Add the error correcting code.
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
rfm22b_dev->tx_packet = p;
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0) {
rfm22b_dev->packet_start_ticks = 1;
}
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
// Queue the data up for sending
rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
RX_LED_OFF;
// Set the destination address in the transmit header.
// The destination address is the first 4 bytes of the message.
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// *******************
// add some data to the chips TX FIFO before enabling the transmitter
// set the total number of data bytes we are going to transmit
rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
// add some data
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
// enable TX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// enable the transmitter
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
TX_LED_ON;
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Receive packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
{
enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS;
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev)) {
return RFM22B_EVENT_FAILURE;
}
// TX FIFO almost empty, it needs filling up
if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) {
// top-up the rf chips TX FIFO buffer
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
// Packet has been sent
} else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) {
portTickType curTicks = xTaskGetTickCount();
rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet);
// Is this an ACK?
bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS));
ret_event = RFM22B_EVENT_RX_MODE;
if (is_ack) {
// If this is an ACK for a connection request message we need to
// configure this modem from the connection request message.
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22_setConnectionParameters(rfm22b_dev);
} else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) {
// Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
// Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) {
PHStatusPacketHandle status = (PHStatusPacketHandle)&(radio_dev->rx_packet);
uint32_t source_id = status->source_id;
for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) {
if (rfm22b_dev->bindings[i].pairID == source_id) {
rfm22b_dev->cur_binding = i;
ret_event = RFM22B_EVENT_REQUEST_CONNECTION;
if (radio_dev->bindings[i].pairID == source_id) {
radio_dev->cur_binding = i;
ret_event = RADIO_EVENT_REQUEST_CONNECTION;
break;
}
}
}
break;
// Change the channel
// On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message.
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22b_dev->time_delta = portMAX_DELAY - curTicks;
case PACKET_TYPE_CON_REQUEST:
ret_event = RADIO_EVENT_CONNECTION_REQUESTED;
break;
case PACKET_TYPE_DATA:
{
// Send the data to the com port
bool rx_need_yield;
if (radio_dev->rx_in_cb)
(radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield);
break;
}
case PACKET_TYPE_DUPLICATE_DATA:
break;
case PACKET_TYPE_ACK:
ret_event = RADIO_EVENT_PACKET_ACKED;
break;
case PACKET_TYPE_NACK:
ret_event = RADIO_EVENT_PACKET_NACKED;
break;
case PACKET_TYPE_PPM:
{
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
PHPpmPacketHandle ppmp = (PHPpmPacketHandle)p;
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
bool ppm_output = false;
#endif
#endif
#if defined(PIOS_INCLUDE_RFM22B_RCVR)
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
radio_dev->ppm_channel[i] = ppmp->channels[i];
}
#endif
#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
if (PIOS_PPM_OUTPUT) {
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
}
}
#endif
#if defined(PIOS_INCLUDE_GCSRCVR)
if (!ppm_output) {
GCSReceiverData gcsRcvr;
for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
gcsRcvr.Channel[i] = ppmp->channels[i];
}
GCSReceiverSet(&gcsRcvr);
}
#endif
break;
}
default:
break;
}
uint16_t seq_num = radio_dev->rx_packet.header.seq_num;
if (rfm22_isConnected(radio_dev)) {
// Adjust the clock syncronization if this is the remote modem.
// The coordinator sends the time that the previous packet was finised transmitting,
// which should match the time that the last packet was received.
uint16_t prev_seq_num = radio_dev->stats.rx_seq;
if (seq_num == (prev_seq_num + 1)) {
portTickType local_rx_time = radio_dev->rx_complete_ticks;
portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time;
radio_dev->time_delta = remote_tx_time - local_rx_time;
} else if (seq_num > prev_seq_num) {
// Add any missed packets into the stats.
uint16_t missed_packets = seq_num - prev_seq_num - 1;
radio_dev->stats.rx_missed += missed_packets;
}
} else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) {
// We need to wait for an ACK if this packet it not an ACK or NACK.
rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet;
rfm22b_dev->tx_complete_ticks = xTaskGetTickCount();
}
// Set the Tx period
if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) {
rfm22b_dev->time_to_send_offset = curTicks + 0x4;
} else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) {
rfm22b_dev->time_to_send_offset = curTicks;
}
rfm22b_dev->tx_packet = 0;
rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
// Start a new transaction
rfm22b_dev->packet_start_ticks = 0;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D1_LED_OFF;
#endif
// Update the sequence number
radio_dev->stats.rx_seq = seq_num;
} else {
ret_event = RADIO_EVENT_RX_COMPLETE;
}
// Log the time that the packet was received.
radio_dev->rx_complete_ticks = curTicks;
if (radio_dev->rx_complete_ticks == 0)
radio_dev->rx_complete_ticks = 1;
return ret_event;
}
/**
* Receive the packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev)
{
enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev);
switch (res) {
case PIOS_RFM22B_RX_COMPLETE:
// Receive the packet.
ret_event = radio_receivePacket(radio_dev, radio_dev->rx_packet_handle, radio_dev->rx_buffer_wr);
radio_dev->rx_buffer_wr = 0;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_OFF;
#endif
// Start a new transaction
radio_dev->packet_start_ticks = 0;
break;
case PIOS_RFM22B_INT_FAILURE:
ret_event = RADIO_EVENT_RX_MODE;
break;
default:
// do nothing.
break;
}
return ret_event;
}
/*****************************************************************************
* Packet Transmition Functions
@ -2026,8 +2130,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
*/
static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
// The coordinator doesn't send status.
if (rfm22b_dev->coordinator) {
// Don't send if a status is already queued.
if (rfm22b_dev->send_status) {
return;
}
@ -2057,7 +2161,7 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
*
* @param[in] rfm22b_dev The device structure
*/
static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
{
#ifdef PIOS_PPM_RECEIVER
// Only send PPM if we're connected
@ -2072,9 +2176,9 @@ static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
// See if we have any valid channels.
bool valid_input_detected = false;
for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) {
rfm22b_dev->ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT)
for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) {
rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1);
if((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT))
valid_input_detected = true;
}
@ -2092,46 +2196,44 @@ static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
* Send an ACK to a received packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
{
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks);
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
// We don't ACK PPM or status packets.
if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) {
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = PACKET_TYPE_ACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
}
return RADIO_EVENT_TX_START;
}
/**
* Send an NACK to a received packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
{
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = PACKET_TYPE_NACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Send a connection request message.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
{
PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet);
@ -2144,15 +2246,15 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
cph->header.type = PACKET_TYPE_CON_REQUEST;
cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet));
cph->source_id = rfm22b_dev->deviceID;
cph->status_rx_time = rfm22b_dev->rx_complete_ticks;
cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port;
cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port;
cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port;
cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed;
rfm22b_dev->time_to_send = true;
rfm22b_dev->send_connection_request = true;
rfm22b_dev->prev_tx_packet = NULL;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
@ -2164,12 +2266,11 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
* Receive an ACK.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
{
PHPacketHandle prev = rfm22b_dev->prev_tx_packet;
portTickType curTicks = xTaskGetTickCount();
// Clear the previous TX packet.
rfm22b_dev->prev_tx_packet = NULL;
@ -2184,36 +2285,17 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de
break;
}
// On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message.
if (prev->header.type == PACKET_TYPE_CON_REQUEST) {
rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks;
} else if (!rfm22b_dev->coordinator) {
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet));
portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks);
portTickType remote_rx_time = aph->packet_recv_time;
// Adjust the time delta based on the difference between our estimated time offset and the coordinator offset.
// This is not working yet
rfm22b_dev->time_delta += remote_rx_time - local_tx_time;
}
// Should we try to start another TX?
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) {
rfm22b_dev->time_to_send_offset = curTicks;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
} else {
rfm22b_dev->time_to_send_offset = curTicks + 0x4;
return RFM22B_EVENT_RX_MODE;
}
return RADIO_EVENT_TX_START;
}
/**
* Receive an MACK.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
{
// Resend the previous TX packet.
@ -2222,19 +2304,18 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d
// Increment the reset packet counter if we're connected.
if (rfm22_isConnected(rfm22b_dev)) {
rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET);
rfm22b_add_rx_status(rfm22b_dev, RADIO_RESENT_TX_PACKET);
}
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Receive a status packet
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
int8_t rssi = rfm22b_dev->rssi_dBm;
@ -2273,7 +2354,7 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b
rfm22b_dev->pair_stats[min_idx].lastContact = 0;
}
return RFM22B_EVENT_RX_COMPLETE;
return RADIO_EVENT_RX_COMPLETE;
}
@ -2297,16 +2378,16 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
uint32_t val = rfm22b_dev->rx_packet_stats[i];
for (uint8_t j = 0; j < 16; ++j) {
switch ((val >> (j * 2)) & 0x3) {
case RFM22B_GOOD_RX_PACKET:
case RADIO_GOOD_RX_PACKET:
rfm22b_dev->stats.rx_good++;
break;
case RFM22B_CORRECTED_RX_PACKET:
case RADIO_CORRECTED_RX_PACKET:
rfm22b_dev->stats.rx_corrected++;
break;
case RFM22B_ERROR_RX_PACKET:
case RADIO_ERROR_RX_PACKET:
rfm22b_dev->stats.rx_error++;
break;
case RFM22B_RESENT_TX_PACKET:
case RADIO_RESENT_TX_PACKET:
rfm22b_dev->stats.tx_resent++;
break;
}
@ -2335,39 +2416,6 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r
rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status;
}
/**
* Is it this modem's turn to send?
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev)
{
// Is there a status of PPM packet ready to send?
if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) {
return true;
}
// Are we not connected yet?
if (!rfm22_isConnected(rfm22b_dev)) {
return true;
}
// Is there some data ready to sent?
PHPacketHandle dp = &rfm22b_dev->data_packet;
if (dp->header.data_size > 0) {
return true;
}
bool need_yield = false;
if (rfm22b_dev->tx_out_cb) {
dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield);
}
if (dp->header.data_size > 0) {
return true;
}
return false;
}
/*****************************************************************************
* Connection Handling Functions
@ -2403,16 +2451,16 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev)
// Configure this modem from the connection request message.
rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing);
rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
pios_rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
}
/**
* Accept a connection request.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
{
// Set our connection state to connected
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
@ -2425,7 +2473,13 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
// Set the destination ID to the source of the connection request message.
rfm22b_dev->destination_id = cph->source_id;
return RFM22B_EVENT_DEFAULT;
// The remote modem sets the time delta between the two modems using the differene between the clock
// on the local modem when it sent the status packet and the time on the coordinator modem when it was received.
portTickType local_tx_time = rfm22b_dev->tx_complete_ticks;
portTickType remote_rx_time = cph->status_rx_time;
rfm22b_dev->time_delta = remote_rx_time - local_tx_time;
return RADIO_EVENT_DEFAULT;
}
@ -2433,20 +2487,6 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
* Frequency Hopping Functions
*****************************************************************************/
/**
* There needs to be a buffer in time around a channel change in which we delay starting a new packet transmit.
* This function returns true of we are in that range of time.
*
* @param[in] rfm22b_dev The device structure
* @return True if we're near a channel change time.
*/
static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
{
portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
uint8_t window = (uint8_t)(time & 0x7e);
return ((window == 0x7f) || (window == 0));
}
/**
* Return the extimated current clock ticks count on the coordinator modem.
* This is the master clock used for all synchronization.
@ -2455,9 +2495,26 @@ static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
*/
static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks)
{
if (rfm22b_dev->coordinator) {
return ticks;
}
return ticks + rfm22b_dev->time_delta;
}
/**
* Return true if this modem is in the send interval, which allows the modem to initiate a transmit.
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
{
portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
// Divide time into 8ms blocks. Coordinator sends in firs 2 ms, and remote send in 5th and 6th ms.
bool tts = (rfm22b_dev->coordinator) ? ((time & 0x06) == 0) : (((time + 4) & 0x06) == 0);
// Noone starts a transmit just prior to a channel change.
return tts && ((time & 0x7e) < 0x7b);
}
/**
* Calculate what the current channel shold be.
*
@ -2492,44 +2549,27 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev)
* Error Handling Functions
*****************************************************************************/
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
rfm22b_dev->in_rx_mode = false;
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
return RFM22B_EVENT_RX_MODE;
}
/**
* Recover from a transmit failure.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.tx_failure++;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Recover from a timeout event.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.timeouts++;
rfm22b_dev->packet_start_ticks = 0;
@ -2537,29 +2577,30 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
if (rfm22b_dev->tx_packet != 0) {
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
}
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
rfm22b_dev->rx_buffer_wr = 0;
TX_LED_OFF;
RX_LED_OFF;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
D2_LED_OFF;
D3_LED_OFF;
D4_LED_OFF;
#endif
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_RX_MODE;
}
/**
* Recover from a severe error.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.resets++;
rfm22_clearLEDs();
return RFM22B_EVENT_INITIALIZE;
return RADIO_EVENT_INITIALIZE;
}
/**
@ -2567,9 +2608,9 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
* this should not happen.
*
* @parem [in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
{
// RF module error .. flash the LED's
rfm22_clearLEDs();
@ -2593,7 +2634,7 @@ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_d
PIOS_Assert(0);
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
@ -2659,7 +2700,7 @@ static void rfm22_clearLEDs(void) {
LINK_LED_OFF;
RX_LED_OFF;
TX_LED_OFF;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
D2_LED_OFF;
D3_LED_OFF;
@ -2728,7 +2769,7 @@ static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev)
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
{
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
@ -2739,22 +2780,18 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_
}
/**
* Read a byte from an RFM22b register
* Write a byte to a register without claiming the semaphore
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
* @param[in] addr The address to read from
* @return Returns the result of the register read
* @param[in] rfm22b_dev The RFM22B device.
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
{
uint8_t in[2];
uint8_t out[2] = {addr & 0x7f, 0xFF};
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
uint8_t buf[2] = {addr | 0x80, data};
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
return in[1];
}
/**
@ -2764,7 +2801,7 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
* @param[in] addr The address to read from
* @return Returns the result of the register read
*/
static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
{
uint8_t out[2] = {addr & 0x7F, 0xFF};
uint8_t in[2];

5
flight/pios/common/pios_rfm22b_com.c
View File

@ -91,7 +91,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud)
* @param[in] rfm22b_dev The device ID.
* @param[in] rx_bytes_available The number of bytes available to receive
*/
static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id,
__attribute__((unused)) uint16_t rx_bytes_avail)
{
}
@ -101,7 +102,7 @@ static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
* @param[in] rfm22b_dev The device ID.
* @param[in] tx_bytes_available The number of bytes available to transmit
*/
static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {

511
flight/pios/common/pios_video.c
View File

@ -33,8 +33,15 @@
#ifdef PIOS_INCLUDE_VIDEO
extern xSemaphoreHandle osdSemaphore;
// Private methods
static void configure_hsync_timers();
static void stop_hsync_timers();
static void reset_hsync_timers();
static void prepare_line(uint32_t line_num);
static void flush_spi();
// Private variables
extern xSemaphoreHandle osdSemaphore;
static const struct pios_video_cfg * dev_cfg;
// Define the buffers.
@ -46,11 +53,11 @@ static const struct pios_video_cfg * dev_cfg;
// Must be allocated in one block, so it is in a struct.
struct _buffers
{
uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
} buffers;
uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
}buffers;
// Remove the struct definition (makes it easier to write for.)
#define buffer0_level (buffers.buffer0_level)
@ -69,6 +76,7 @@ volatile uint16_t gActiveLine = 0;
volatile uint16_t gActivePixmapLine = 0;
volatile uint16_t line=0;
volatile uint16_t Vsync_update=0;
volatile uint16_t Hsync_update=0;
static int16_t m_osdLines=0;
/**
@ -78,204 +86,395 @@ static int16_t m_osdLines=0;
*/
void swap_buffers()
{
// While we could use XOR swap this is more reliable and
// dependable and it's only called a few times per second.
// Many compliers should optimise these to EXCH instructions.
uint8_t *tmp;
SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask);
SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level);
// While we could use XOR swap this is more reliable and
// dependable and it's only called a few times per second.
// Many compliers should optimise these to EXCH instructions.
uint8_t *tmp;
SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask);
SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level);
}
bool PIOS_Hsync_ISR() {
if(dev_cfg->hsync->pin.gpio->IDR & dev_cfg->hsync->pin.init.GPIO_Pin) {
//rising
if(gLineType == LINE_TYPE_GRAPHICS)
{
// Activate new line
DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE);
DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE);
}
} else {
//falling
gLineType = LINE_TYPE_UNKNOWN; // Default case
gActiveLine++;
if ((gActiveLine >= GRAPHICS_LINE) && (gActiveLine < (GRAPHICS_LINE + GRAPHICS_HEIGHT))) {
gLineType = LINE_TYPE_GRAPHICS;
gActivePixmapLine = (gActiveLine - GRAPHICS_LINE);
line = gActivePixmapLine*GRAPHICS_WIDTH;
}
if(gLineType == LINE_TYPE_GRAPHICS)
{
// Load new line
DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[line],DMA_Memory_0);
DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[line],DMA_Memory_0);
DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel,BUFFER_LINE_LENGTH);
DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel,BUFFER_LINE_LENGTH);
}
}
return false;
bool PIOS_Hsync_ISR()
{
// On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1
if(Hsync_update==GRAPHICS_LINE) {
prepare_line(0);
gActiveLine = 1;
}
Hsync_update++;
return true;
}
bool PIOS_Vsync_ISR() {
static portBASE_TYPE xHigherPriorityTaskWoken;
//PIOS_LED_Toggle(LED3);
static portBASE_TYPE xHigherPriorityTaskWoken;
//if(gActiveLine > 200)
xHigherPriorityTaskWoken = pdFALSE;
m_osdLines = gActiveLine;
{
gActiveLine = 0;
Vsync_update++;
if(Vsync_update>=2)
{
swap_buffers();
Vsync_update=0;
xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken);
}
}
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
m_osdLines = gActiveLine;
return xHigherPriorityTaskWoken == pdTRUE;
stop_hsync_timers();
// Wait for previous word to clock out of each
TIM_Cmd(dev_cfg->pixel_timer.timer, ENABLE);
flush_spi();
TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
gActiveLine = 0;
Hsync_update = 0;
Vsync_update++;
if(Vsync_update>=2)
{
// load second image buffer
swap_buffers();
Vsync_update=0;
// trigger redraw every second field
xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken);
}
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
return xHigherPriorityTaskWoken == pdTRUE;
}
uint16_t PIOS_Video_GetOSDLines(void) {
return m_osdLines;
return m_osdLines;
}
void PIOS_Video_Init(const struct pios_video_cfg * cfg){
/**
* Stops the pixel clock and ensures it ignores the rising edge. To be used after a
* vsync until the first line is to be displayed
*/
static void stop_hsync_timers()
{
// This removes the slave mode configuration
TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
TIM_InternalClockConfig(dev_cfg->pixel_timer.timer);
}
dev_cfg = cfg; // store config before enabling interrupt
const struct pios_tim_callbacks px_callback = {
.overflow = NULL,
.edge = NULL,
};
if (cfg->mask.remap) {
GPIO_PinAFConfig(cfg->mask.sclk.gpio,
__builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
cfg->mask.remap);
GPIO_PinAFConfig(cfg->mask.mosi.gpio,
__builtin_ctz(cfg->mask.mosi.init.GPIO_Pin),
cfg->mask.remap);
}
if (cfg->level.remap)
{
GPIO_PinAFConfig(cfg->level.sclk.gpio,
__builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
cfg->level.remap);
GPIO_PinAFConfig(cfg->level.miso.gpio,
__builtin_ctz(cfg->level.miso.init.GPIO_Pin),
cfg->level.remap);
}
#ifdef PAL
const uint32_t period = 10;
const uint32_t dc = (10 / 2);
#else
const uint32_t period = 11;
const uint32_t dc = (11 / 2);
#endif
/**
* Reset the timer and configure for next call. Keeps them synced. Ideally this won't even be needed
* since I don't think the slave mode gets lost, and this can simply be disable timer
*/
uint32_t failcount = 0;
static void reset_hsync_timers()
{
// Stop both timers
TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
/* SPI3 MASTER MASKBUFFER */
GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
GPIO_Init(cfg->mask.mosi.gpio, (GPIO_InitTypeDef*)&(cfg->mask.mosi.init));
uint32_t tim_id;
const struct pios_tim_channel *channels = &dev_cfg->hsync_capture;
/* SPI1 SLAVE FRAMEBUFFER */
GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init));
GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init));
//BUG: This is nuts this line is needed. It simply results in allocating
//all the memory but somehow leaving it out breaks the timer functionality.
// I do not see how these can be related
if (failcount == 0) {
if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0)
failcount++;
}
/* Initialize the SPI block */
SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc;
/* Enable SPI */
SPI_Cmd(cfg->level.regs, ENABLE);
SPI_Cmd(cfg->mask.regs, ENABLE);
// Listen to Channel1 (HSYNC)
switch(dev_cfg->hsync_capture.timer_chan) {
case TIM_Channel_1:
TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1);
break;
case TIM_Channel_2:
TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2);
break;
default:
PIOS_Assert(0);
}
TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger);
}
/* Configure DMA for SPI Tx MASTER */
DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init));
static void configure_hsync_timers()
{
// Stop both timers
TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
/* Configure DMA for SPI Tx SLAVE */
DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init));
// This is overkill but used for consistency. No interrupts used for pixel clock
// but this function calls the GPIO_Remap
uint32_t tim_id;
const struct pios_tim_channel *channels;
// Init the channel to output the pixel clock
channels = &dev_cfg->pixel_timer;
PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0);
/* Trigger interrupt when for half conversions too to indicate double buffer */
DMA_ITConfig(cfg->mask.dma.tx.channel, DMA_IT_TC, ENABLE);
/*DMA_ClearFlag(cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5);
DMA_ClearITPendingBit(cfg->mask.dma.tx.channel, DMA_IT_TCIF5);
// Init the channel to capture the pulse
channels = &dev_cfg->hsync_capture;
PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0);
DMA_ClearFlag(cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
DMA_ClearITPendingBit(cfg->level.dma.tx.channel, DMA_IT_TCIF5);
*/
// Configure the input capture channel
TIM_ICInitTypeDef TIM_ICInitStructure;
switch(dev_cfg->hsync_capture.timer_chan) {
case TIM_Channel_1:
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
break;
case TIM_Channel_2:
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
break;
default:
PIOS_Assert(0);
}
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
//TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0;
TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure);
/* Configure DMA interrupt */
NVIC_Init(&cfg->level.dma.irq.init);
NVIC_Init(&cfg->mask.dma.irq.init);
// Set up the channel to output the pixel clock
switch(dev_cfg->pixel_timer.timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc);
break;
case TIM_Channel_2:
TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc);
break;
case TIM_Channel_3:
TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc);
break;
case TIM_Channel_4:
TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc);
break;
}
TIM_ARRPreloadConfig(dev_cfg->pixel_timer.timer, ENABLE);
TIM_CtrlPWMOutputs(dev_cfg->pixel_timer.timer, ENABLE);
/* Enable SPI interrupts to DMA */
SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
// This shouldn't be needed as it should come from the config struture. Something
// is clobbering that
TIM_PrescalerConfig(dev_cfg->pixel_timer.timer, 0, TIM_PSCReloadMode_Immediate);
TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period);
}
/* Configure the Video Line interrupt */
PIOS_EXTI_Init(cfg->hsync);
PIOS_EXTI_Init(cfg->vsync);
DMA_TypeDef * main_dma;
DMA_TypeDef * mask_dma;
DMA_Stream_TypeDef * main_stream;
DMA_Stream_TypeDef * mask_stream;
void PIOS_Video_Init(const struct pios_video_cfg * cfg)
{
dev_cfg = cfg; // store config before enabling interrupt
configure_hsync_timers();
/* needed for HW hack */
const GPIO_InitTypeDef initStruct = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN ,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
};
GPIO_Init(GPIOC, &initStruct);
/* SPI3 - MASKBUFFER */
GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init));
/* SPI1 SLAVE FRAMEBUFFER */
GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init));
GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init));
if (cfg->mask.remap) {
GPIO_PinAFConfig(cfg->mask.sclk.gpio,
__builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
cfg->mask.remap);
GPIO_PinAFConfig(cfg->mask.miso.gpio,
__builtin_ctz(cfg->mask.miso.init.GPIO_Pin),
cfg->mask.remap);
}
if (cfg->level.remap)
{
GPIO_PinAFConfig(cfg->level.sclk.gpio,
__builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
cfg->level.remap);
GPIO_PinAFConfig(cfg->level.miso.gpio,
__builtin_ctz(cfg->level.miso.init.GPIO_Pin),
cfg->level.remap);
}
/* Initialize the SPI block */
SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
/* Enable SPI */
SPI_Cmd(cfg->level.regs, ENABLE);
SPI_Cmd(cfg->mask.regs, ENABLE);
/* Configure DMA for SPI Tx SLAVE Maskbuffer */
DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init));
/* Configure DMA for SPI Tx SLAVE Framebuffer*/
DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init));
/* Trigger interrupt when for half conversions too to indicate double buffer */
DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE);
/* Configure and clear buffers */
draw_buffer_level = buffer0_level;
draw_buffer_mask = buffer0_mask;
disp_buffer_level = buffer1_level;
disp_buffer_mask = buffer1_mask;
memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
/* Configure DMA interrupt */
NVIC_Init(&cfg->level.dma.irq.init);
/* Enable SPI interrupts to DMA */
SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
mask_dma = DMA1;
main_dma = DMA2;
main_stream = cfg->level.dma.tx.channel;
mask_stream = cfg->mask.dma.tx.channel;
/* Configure the Video Line interrupt */
PIOS_EXTI_Init(cfg->hsync);
PIOS_EXTI_Init(cfg->vsync);
//set levels to zero
PIOS_Servo_Set(0,0);
PIOS_Servo_Set(1,0);
}
/**
* @brief Interrupt for half and full buffer transfer
*
* This interrupt handler swaps between the two halfs of the double buffer to make
* sure the ahrs uses the most recent data. Only swaps data when AHRS is idle, but
* really this is a pretense of a sanity check since the DMA engine is consantly
* running in the background. Keep an eye on the ekf_too_slow variable to make sure
* it's keeping up.
* Prepare the system to watch for a HSYNC pulse to trigger the pixel
* clock and clock out the next line
*/
static void prepare_line(uint32_t line_num)
{
if(line_num<GRAPHICS_HEIGHT)
{
uint32_t buf_offset = line_num * GRAPHICS_WIDTH;
dev_cfg->pixel_timer.timer->CNT = dc;
DMA_ClearFlag(dev_cfg->mask.dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7);
DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5);
// Load new line
DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0);
DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0);
// Enable DMA, Slave first
DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH);
DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel, BUFFER_LINE_LENGTH);
SPI_Cmd(dev_cfg->level.regs, ENABLE);
SPI_Cmd(dev_cfg->mask.regs, ENABLE);
/* Enable SPI interrupts to DMA */
SPI_I2S_DMACmd(dev_cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
SPI_I2S_DMACmd(dev_cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE);
DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE);
}
reset_hsync_timers();
}
void PIOS_VIDEO_DMA_Handler(void);
void DMA1_Stream7_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler")));
void DMA2_Stream5_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler")));
/**
* Check both SPI for the stop sequence before disabling them
*/
static void flush_spi()
{
bool level_empty = false;
bool mask_empty = false;
bool level_stopped = false;
bool mask_stopped = false;
// Can't flush if clock not running
while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) {
level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET;
mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET;
if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) {
SPI_Cmd(dev_cfg->level.regs, DISABLE);
level_stopped = true;
}
if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == RESET) {
SPI_Cmd(dev_cfg->mask.regs, DISABLE);
mask_stopped = true;
}
}
/*
uint32_t i = 0;
while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++;
while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++;
while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;
while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/
SPI_Cmd(dev_cfg->mask.regs, DISABLE);
SPI_Cmd(dev_cfg->level.regs, DISABLE);
}
/**
* @brief Interrupt for half and full buffer transfer
*/
void PIOS_VIDEO_DMA_Handler(void)
{
if (DMA_GetFlagStatus(DMA1_Stream7,DMA_FLAG_TCIF7)) { // transfer completed load next line
DMA_ClearFlag(DMA1_Stream7,DMA_FLAG_TCIF7);
//PIOS_LED_Off(LED2);
/*if(gLineType == LINE_TYPE_GRAPHICS)
{
// Load new line
DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[line],DMA_Memory_0);
DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[line],DMA_Memory_0);
//DMA_ClearFlag(dev_cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5); // <-- TODO: HARDCODED
//DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); // <-- TODO: HARDCODED
DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel,BUFFER_LINE_LENGTH);
DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel,BUFFER_LINE_LENGTH);
}*/
//PIOS_LED_Toggle(LED2);
}
else if (DMA_GetFlagStatus(DMA1_Stream7,DMA_FLAG_HTIF7)) {
DMA_ClearFlag(DMA1_Stream7,DMA_FLAG_HTIF7);
}
else {
// Handle flags from stream channel
if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled
DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
if(gActiveLine < GRAPHICS_HEIGHT)
{
flush_spi();
stop_hsync_timers();
}
dev_cfg->pixel_timer.timer->CNT = dc;
if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_TCIF5)) { // whole double buffer filled
DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_TCIF5);
//PIOS_LED_Toggle(LED3);
}
else if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_HTIF5)) {
DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_HTIF5);
}
else {
}
prepare_line(gActiveLine);
}
else if(gActiveLine >= GRAPHICS_HEIGHT)
{
//last line completed
flush_spi();
stop_hsync_timers();
// STOP DMA, master first
DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
}
gActiveLine++;
}
else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) {
DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5);
}
else {
}
}
#endif /* PIOS_INCLUDE_VIDEO */

1
flight/pios/inc/pios_hcsr04.h
View File

@ -32,7 +32,6 @@
#define PIOS_HCSR04_H
/* Public Functions */
extern void PIOS_HCSR04_Init(void);
extern int32_t PIOS_HCSR04_Get(void);
extern int32_t PIOS_HCSR04_Completed(void);
extern void PIOS_HCSR04_Trigger(void);

55
flight/pios/inc/pios_hcsr04_priv.h Normal file
View File

@ -0,0 +1,55 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SERVO Servo Functions
* @brief PIOS interface to read and write from servo PWM ports
* @{
*
* @file pios_servo_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Servo private structures.
* @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_HCSR04_PRIV_H
#define PIOS_HCSR04_PRIV_H
#include <pios.h>
#include <pios_stm32.h>
#include <pios_tim_priv.h>
struct pios_hcsr04_cfg {
TIM_ICInitTypeDef tim_ic_init;
const struct pios_tim_channel * channels;
uint8_t num_channels;
struct stm32_gpio trigger;
};
extern const struct pios_rcvr_driver pios_pwm_rcvr_driver;
extern int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg);
#endif /* PIOS_HCSR04_PRIV_H */
/**
* @}
* @}
*/

12
flight/pios/inc/pios_rfm22b.h
View File

@ -75,6 +75,13 @@ enum rfm22b_datarate {
RFM22_datarate_256000 = 14,
};
typedef enum {
PIOS_RFM22B_INT_FAILURE,
PIOS_RFM22B_INT_SUCCESS,
PIOS_RFM22B_TX_COMPLETE,
PIOS_RFM22B_RX_COMPLETE
} pios_rfm22b_int_result;
struct rfm22b_stats {
uint16_t packets_per_sec;
uint16_t tx_byte_count;
@ -116,7 +123,12 @@ extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id);
extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);
extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs);
extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id);
extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id);
extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p);
extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p);
extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id);
extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id);
/* Global Variables */
extern const struct pios_com_driver pios_rfm22b_com_driver;

205
flight/pios/inc/pios_rfm22b_priv.h
View File

@ -531,62 +531,69 @@ enum pios_rfm22b_dev_magic {
PIOS_RFM22B_DEV_MAGIC = 0x68e971b6,
};
enum pios_radio_state {
RADIO_STATE_UNINITIALIZED,
RADIO_STATE_INITIALIZING,
RADIO_STATE_REQUESTING_CONNECTION,
RADIO_STATE_ACCEPTING_CONNECTION,
RADIO_STATE_RX_MODE,
RADIO_STATE_RX_DATA,
RADIO_STATE_RX_FAILURE,
RADIO_STATE_RECEIVING_STATUS,
RADIO_STATE_TX_START,
RADIO_STATE_TX_DATA,
RADIO_STATE_TX_FAILURE,
RADIO_STATE_SENDING_ACK,
RADIO_STATE_SENDING_NACK,
RADIO_STATE_RECEIVING_ACK,
RADIO_STATE_RECEIVING_NACK,
RADIO_STATE_TIMEOUT,
RADIO_STATE_ERROR,
RADIO_STATE_FATAL_ERROR,
RADIO_STATE_NUM_STATES // Must be last
};
enum pios_radio_event {
RADIO_EVENT_DEFAULT,
RADIO_EVENT_INT_RECEIVED,
RADIO_EVENT_INITIALIZE,
RADIO_EVENT_INITIALIZED,
RADIO_EVENT_REQUEST_CONNECTION,
RADIO_EVENT_CONNECTION_REQUESTED,
RADIO_EVENT_PACKET_ACKED,
RADIO_EVENT_PACKET_NACKED,
RADIO_EVENT_ACK_TIMEOUT,
RADIO_EVENT_RX_MODE,
RADIO_EVENT_RX_COMPLETE,
RADIO_EVENT_STATUS_RECEIVED,
RADIO_EVENT_TX_START,
RADIO_EVENT_FAILURE,
RADIO_EVENT_TIMEOUT,
RADIO_EVENT_ERROR,
RADIO_EVENT_FATAL_ERROR,
RADIO_EVENT_NUM_EVENTS // Must be last
};
enum pios_rfm22b_state {
RFM22B_STATE_UNINITIALIZED,
RFM22B_STATE_INITIALIZING,
RFM22B_STATE_REQUESTING_CONNECTION,
RFM22B_STATE_ACCEPTING_CONNECTION,
RFM22B_STATE_TRANSITION,
RFM22B_STATE_RX_WAIT,
RFM22B_STATE_RX_WAIT_SYNC,
RFM22B_STATE_RX_MODE,
RFM22B_STATE_WAIT_PREAMBLE,
RFM22B_STATE_WAIT_SYNC,
RFM22B_STATE_RX_DATA,
RFM22B_STATE_RX_FAILURE,
RFM22B_STATE_RECEIVING_STATUS,
RFM22B_STATE_TX_START,
RFM22B_STATE_TX_DATA,
RFM22B_STATE_TX_FAILURE,
RFM22B_STATE_SENDING_ACK,
RFM22B_STATE_SENDING_NACK,
RFM22B_STATE_RECEIVING_ACK,
RFM22B_STATE_RECEIVING_NACK,
RFM22B_STATE_TIMEOUT,
RFM22B_STATE_ERROR,
RFM22B_STATE_FATAL_ERROR,
RFM22B_STATE_TX_MODE,
RFM22B_STATE_TRANSMITTING,
RFM22B_STATE_NUM_STATES // Must be last
};
enum pios_rfm22b_event {
RFM22B_EVENT_DEFAULT,
RFM22B_EVENT_INT_RECEIVED,
RFM22B_EVENT_INITIALIZE,
RFM22B_EVENT_INITIALIZED,
RFM22B_EVENT_REQUEST_CONNECTION,
RFM22B_EVENT_CONNECTION_REQUESTED,
RFM22B_EVENT_PACKET_ACKED,
RFM22B_EVENT_PACKET_NACKED,
RFM22B_EVENT_ACK_TIMEOUT,
RFM22B_EVENT_RX_MODE,
RFM22B_EVENT_PREAMBLE_DETECTED,
RFM22B_EVENT_SYNC_DETECTED,
RFM22B_EVENT_RX_COMPLETE,
RFM22B_EVENT_RX_ERROR,
RFM22B_EVENT_STATUS_RECEIVED,
RFM22B_EVENT_TX_START,
RFM22B_EVENT_FAILURE,
RFM22B_EVENT_TIMEOUT,
RFM22B_EVENT_ERROR,
RFM22B_EVENT_FATAL_ERROR,
RFM22B_EVENT_NUM_EVENTS // Must be last
};
#define RFM22B_RX_PACKET_STATS_LEN 4
enum pios_rfm22b_rx_packet_status {
RFM22B_GOOD_RX_PACKET = 0x00,
RFM22B_CORRECTED_RX_PACKET = 0x01,
RFM22B_ERROR_RX_PACKET = 0x2,
RFM22B_RESENT_TX_PACKET = 0x3
RADIO_GOOD_RX_PACKET = 0x00,
RADIO_CORRECTED_RX_PACKET = 0x01,
RADIO_ERROR_RX_PACKET = 0x2,
RADIO_RESENT_TX_PACKET = 0x3
};
typedef struct {
@ -604,6 +611,79 @@ typedef struct {
OPLinkSettingsComSpeedOptions com_speed;
} rfm22b_binding;
enum pios_rfm22b_chip_power_state {
RFM22B_IDLE_STATE = 0x00,
RFM22B_RX_STATE = 0x01,
RFM22B_TX_STATE = 0x10,
RFM22B_INVALID_STATE = 0x11
};
// Device Status
typedef union {
struct {
uint8_t state : 2;
bool frequency_error : 1;
bool header_error : 1;
bool rx_fifo_empty : 1;
bool fifo_underflow : 1;
bool fifo_overflow : 1;
};
uint8_t raw;
} rfm22b_device_status_reg;
// EzMAC Status
typedef union {
struct {
bool packet_sent : 1;
bool packet_transmitting : 1;
bool crc_error : 1;
bool valid_packet_received : 1;
bool packet_receiving : 1;
bool packet_searching : 1;
bool crc_is_all_ones : 1;
bool reserved;
};
uint8_t raw;
} rfm22b_ezmac_status_reg;
// Interrrupt Status Register 1
typedef union {
struct {
bool crc_error : 1;
bool valid_packet_received : 1;
bool packet_sent_interrupt : 1;
bool external_interrupt : 1;
bool rx_fifo_almost_full : 1;
bool tx_fifo_almost_empty : 1;
bool tx_fifo_almost_full : 1;
bool fifo_underoverflow_error : 1;
};
uint8_t raw;
} rfm22b_int_status_1;
// Interrupt Status Register 2
typedef union {
struct {
bool poweron_reset : 1;
bool chip_ready : 1;
bool low_battery_detect : 1;
bool wakeup_timer : 1;
bool rssi_above_threshold : 1;
bool invalid_preamble_detected : 1;
bool valid_preamble_detected : 1;
bool sync_word_detected : 1;
};
uint8_t raw;
} rfm22b_int_status_2;
typedef struct {
rfm22b_device_status_reg device_status;
rfm22b_device_status_reg ezmac_status;
rfm22b_int_status_1 int_status_1;
rfm22b_int_status_2 int_status_2;
} rfm22b_device_status;
struct pios_rfm22b_dev {
enum pios_rfm22b_dev_magic magic;
struct pios_rfm22b_cfg cfg;
@ -644,30 +724,27 @@ struct pios_rfm22b_dev {
uint32_t tx_out_context;
// the transmit power to use for data transmissions
uint8_t tx_power;
uint8_t tx_power;
// The RF datarate lookup index.
uint8_t datarate;
// The state machine state and the current event
enum pios_rfm22b_state state;
// The radio state machine state
enum pios_radio_state state;
// The event queue handle
xQueueHandle eventQueue;
// device status register
uint8_t device_status;
// interrupt status register 1
uint8_t int_status1;
// interrupt status register 2
uint8_t int_status2;
// ezmac status register
uint8_t ezmac_status;
// The device status registers.
rfm22b_device_status status_regs;
// The error statistics counters
uint16_t prev_rx_seq_num;
uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN];
// The RFM22B state machine state
enum pios_rfm22b_state rfm22b_state;
// The packet statistics
struct rfm22b_stats stats;
@ -692,12 +769,12 @@ struct pios_rfm22b_dev {
// The rx data packet
PHPacket rx_packet;
// The rx data packet
PHPacketHandle rx_packet_handle;
// The receive buffer write index
uint16_t rx_buffer_wr;
// The receive buffer write index
uint16_t rx_packet_len;
// Is the modem currently in Rx mode?
bool in_rx_mode;
// The status packet
PHStatusPacket status_packet;
@ -717,10 +794,6 @@ struct pios_rfm22b_dev {
bool send_status;
bool send_ppm;
bool send_connection_request;
bool time_to_send;
// The offset between our clock and the global send clock
uint8_t time_to_send_offset;
// The initial frequency
uint32_t init_frequency;
@ -730,7 +803,7 @@ struct pios_rfm22b_dev {
// The frequency hopping step size
float frequency_step_size;
// current frequency hop channel
uint8_t frequency_hop_channel;
uint8_t frequency_hop_channel;
// afc correction reading (in Hz)
int8_t afc_correction_Hz;
@ -749,7 +822,7 @@ struct pios_rfm22b_dev {
#ifdef PIOS_INCLUDE_RFM22B_RCVR
// The PPM channel values
uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ppm_supv_timer;
uint32_t ppm_supv_timer;
bool ppm_fresh;
#endif
};

19
flight/pios/inc/pios_video.h
View File

@ -42,12 +42,10 @@ struct pios_video_cfg {
const struct pios_exti_cfg * hsync;
const struct pios_exti_cfg * vsync;
/*struct stm32_exti hsync;
struct stm32_exti vsync;
struct stm32_gpio hsync_io;
struct stm32_gpio vsync_io;
struct stm32_irq hsync_irq;
struct stm32_irq vsync_irq;*/
struct pios_tim_channel pixel_timer;
struct pios_tim_channel hsync_capture;
TIM_OCInitTypeDef tim_oc_init;
};
// Time vars
@ -65,20 +63,21 @@ extern bool PIOS_Hsync_ISR();
extern bool PIOS_Vsync_ISR();
// First OSD line
#define GRAPHICS_LINE 32
#define GRAPHICS_LINE 25
//top/left deadband
#define GRAPHICS_HDEADBAND 32
#define GRAPHICS_HDEADBAND 80
#define GRAPHICS_VDEADBAND 0
#define PAL
// Real OSD size
#ifdef PAL
#define GRAPHICS_WIDTH_REAL (336+GRAPHICS_HDEADBAND)
//#define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND)
#define GRAPHICS_WIDTH_REAL 416
#define GRAPHICS_HEIGHT_REAL (270+GRAPHICS_VDEADBAND)
#else
#define GRAPHICS_WIDTH_REAL (320+GRAPHICS_HDEADBAND)
#define GRAPHICS_WIDTH_REAL (312+GRAPHICS_HDEADBAND)
#define GRAPHICS_HEIGHT_REAL (225+GRAPHICS_VDEADBAND)
#endif

7
flight/pios/pios.h
View File

@ -51,11 +51,11 @@
#include <math.h>
/* STM32 Std Peripherals Lib */
#ifdef STM32F4XX
#if defined(STM32F10X)
#include <stm32f10x.h>
#elif defined(STM32F4XX)
#include <stm32f4xx.h>
#include <stm32f4xx_rcc.h>
#else
#include <stm32f10x.h>
#endif
/* PIOS board specific feature selection */
@ -67,6 +67,7 @@
/* PIOS debug interface */
/* #define PIOS_INCLUDE_DEBUG_CONSOLE */
/* #define DEBUG_LEVEL 0 */
/* #define PIOS_ENABLE_DEBUG_PINS */
#include <pios_debug.h>
/* PIOS common functions */

31
flight/pios/stm32f10x/pios_debug.c
View File

@ -7,7 +7,7 @@
* @{
*
* @file pios_debug.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Debugging Functions
* @see The GNU Public License (GPL) Version 3
*
@ -41,7 +41,8 @@ static uint8_t debug_num_channels;
/**
* Initialise Debug-features
*/
void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels,
__attribute__((unused)) uint8_t num_channels)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
PIOS_Assert(channels);
@ -60,13 +61,13 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = chan->init->GPIO_Pin;
GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin;
/* Initialize the GPIO */
GPIO_Init(chan->init->port, &GPIO_InitStructure);
GPIO_Init(chan->pin.gpio, &GPIO_InitStructure);
/* Set the pin low */
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET);
}
#endif // PIOS_ENABLE_DEBUG_PINS
}
@ -75,7 +76,7 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
* Set debug-pin high
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinHigh(uint8_t pin)
void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -84,7 +85,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
const struct pios_tim_channel * chan = &debug_channels[pin];
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_Set);
GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET);
#endif // PIOS_ENABLE_DEBUG_PINS
}
@ -93,7 +94,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
* Set debug-pin low
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinLow(uint8_t pin)
void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -102,13 +103,13 @@ void PIOS_DEBUG_PinLow(uint8_t pin)
const struct pios_tim_channel * chan = &debug_channels[pin];
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET);
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue8Bit(uint8_t value)
void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -124,14 +125,14 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value)
* This is sketchy since it assumes a particular ordering
* and bitwise layout of the channels provided to the debug code.
*/
debug_channels[0].init.port->BSRR = bsrr_l;
debug_channels[4].init.port->BSRR = bsrr_h;
debug_channels[0].pin.gpio->BSRR = bsrr_l;
debug_channels[4].pin.gpio->BSRR = bsrr_h;
PIOS_IRQ_Enable();
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue4BitL(uint8_t value)
void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -143,7 +144,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
* and bitwise layout of the channels provided to the debug code.
*/
uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6);
debug_channels[0].init.port->BSRR = bsrr_l;
debug_channels[0].pin.gpio->BSRR = bsrr_l;
#endif // PIOS_ENABLE_DEBUG_PINS
}
@ -151,7 +152,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
/**
* Report a serious error and halt
*/
void PIOS_DEBUG_Panic(const char *msg)
void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg)
{
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
register int *lr asm("lr"); // Link-register holds the PC of the caller

2
flight/pios/stm32f10x/pios_eeprom.c
View File

@ -88,7 +88,7 @@ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len)
// write 4 bytes at a time into program flash area (emulated EEPROM area)
uint8_t *p1 = data;
uint32_t *p3 = (uint32_t *)config.base_address;
for (int32_t i = 0; i < size; p3++)
for (uint32_t i = 0; i < size; p3++)
{
uint32_t value = 0;

2
flight/pios/stm32f10x/pios_led.c
View File

@ -34,7 +34,7 @@
#include <pios_led_priv.h>
const static struct pios_led_cfg * led_cfg;
static const struct pios_led_cfg * led_cfg;
/**
* Initialises all the LED's

15
flight/pios/stm32f10x/pios_ppm.c
View File

@ -114,7 +114,7 @@ static struct pios_ppm_dev * PIOS_PPM_alloc(void)
static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PPM_tim_overflow_cb,
.edge = PIOS_PPM_tim_edge_cb,
};
@ -188,6 +188,7 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
ppm_dev->supv_timer = 0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0);
}
@ -223,7 +224,10 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
return ppm_dev->CaptureValue[channel];
}
static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
__attribute__((unused)) uint8_t channel,
uint16_t count)
{
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -238,7 +242,10 @@ static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
}
static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
uint8_t chan_idx,
uint16_t count)
{
/* Recover our device context */
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -286,7 +293,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha
/* Check if the last frame was well formed */
if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
/* The last frame was well formed */
for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
for (int32_t i = 0; i < ppm_dev->NumChannels; i++) {
ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
}
for (uint32_t i = ppm_dev->NumChannels;

131
flight/pios/stm32f10x/pios_ppm_out.c
View File

@ -49,16 +49,20 @@ struct pios_ppm_out_dev {
enum pios_ppm_out_dev_magic magic;
const struct pios_ppm_out_cfg * cfg;
uint32_t triggering_period;
uint32_t TriggeringPeriod;
uint32_t ChannelSum;
uint8_t NumChannelCounter;
uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS];
uint8_t supv_timer;
bool Tracking;
uint8_t SupvTimer;
bool Fresh;
bool Tracking;
bool Enabled;
};
static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id);
static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable);
static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev)
{
return (ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC);
@ -94,7 +98,7 @@ static struct pios_ppm_out_dev * PIOS_PPM_alloc(void)
#endif
static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
const static struct pios_tim_callbacks tim_out_callbacks = {
static const struct pios_tim_callbacks tim_out_callbacks = {
.overflow = NULL,
.edge = PIOS_PPM_OUT_tim_edge_cb,
};
@ -115,12 +119,12 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
ppm_dev->cfg = cfg;
// Set up the state variables
ppm_dev->triggering_period = PIOS_PPM_OUT_HIGH_PULSE_US;
ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US;
ppm_dev->ChannelSum = 0;
ppm_dev->NumChannelCounter = 0;
// Flush counter variables
for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; i++)
for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i)
ppm_dev->ChannelValue[i] = 1000;
uint32_t tim_id;
@ -149,20 +153,6 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_2:
TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_3:
TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_4:
TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE);
break;
}
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
@ -174,20 +164,17 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1);
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
switch(chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, ppm_dev->triggering_period);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, ppm_dev->triggering_period);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, ppm_dev->triggering_period);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, ppm_dev->triggering_period);
break;
PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
// Configure the supervisor
ppm_dev->SupvTimer = 0;
ppm_dev->Fresh = FALSE;
ppm_dev->Tracking = FALSE;
ppm_dev->Enabled = FALSE;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0);
}
return 0;
}
@ -202,24 +189,52 @@ void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position)
position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US;
if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US)
position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US;
// Update the supervisor tracking variables.
ppm_dev->Fresh = TRUE;
// Reenable the TIM if it's been turned off.
if (!ppm_dev->Tracking) {
ppm_dev->Tracking = TRUE;
PIOS_PPM_Out_Enable_Disable(ppm_dev, true);
}
// Update the position
ppm_dev->ChannelValue[servo] = position;
}
static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PPM_OUT_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
__attribute__((unused)) uint8_t chan_idx,
__attribute__((unused)) uint16_t count)
{
struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context;
if (!PIOS_PPM_Out_validate(ppm_dev))
return;
// Just return if the device is disabled.
if (!ppm_dev->Enabled) {
return;
}
// Turn off the PPM stream if we are no longer receiving update
// Note: This must happen between frames.
if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) {
// Flush counter variables
for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) {
ppm_dev->ChannelValue[i] = 1000;
}
PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
return;
}
// Finish out the frame if we reached the last channel.
uint32_t pulse_width;
if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) {
pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum;
ppm_dev->NumChannelCounter = 0;
ppm_dev->ChannelSum = 0;
} else
} else
ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]);
// Initiate the pulse
@ -228,4 +243,50 @@ static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t
return;
}
static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable)
{
const struct pios_tim_channel *chan = ppm_dev->cfg->channel;
uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0;
FunctionalState state = enable ? ENABLE : DISABLE;
ppm_dev->Enabled = enable;
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state);
TIM_SetCompare1(chan->timer, trig);
break;
case TIM_Channel_2:
TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state);
TIM_SetCompare2(chan->timer, trig);
break;
case TIM_Channel_3:
TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state);
TIM_SetCompare3(chan->timer, trig);
break;
case TIM_Channel_4:
TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state);
TIM_SetCompare4(chan->timer, trig);
break;
}
}
static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) {
struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id;
if (!PIOS_PPM_Out_validate(ppm_dev))
return;
// RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz.
if(++(ppm_dev->SupvTimer) < 50) {
return;
}
ppm_dev->SupvTimer = 0;
// Go into failsafe the channel values haven't been refreshed since the last time through.
if (!ppm_dev->Fresh) {
ppm_dev->Tracking = FALSE;
}
// Set Fresh to false to test if channel values are being refreshed.
ppm_dev->Fresh = FALSE;
}
#endif /* PIOS_INCLUDE_PPM_OUT */

8
flight/pios/stm32f10x/pios_pwm.c
View File

@ -44,7 +44,7 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
/* Local Variables */
/* 100 ms timeout without updates on channels */
const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
enum pios_pwm_dev_magic {
PIOS_PWM_DEV_MAGIC = 0xab30293c,
@ -98,7 +98,7 @@ static struct pios_pwm_dev * PIOS_PWM_alloc(void)
static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PWM_tim_overflow_cb,
.edge = PIOS_PWM_tim_edge_cb,
};
@ -193,7 +193,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
return pwm_dev->CaptureValue[channel];
}
static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
{
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
@ -219,7 +219,7 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
return;
}
static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;

2
flight/pios/stm32f10x/pios_spi.c
View File

@ -39,7 +39,7 @@
#include <pios_spi_priv.h>
static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev)
{
/* Should check device magic here */
return(true);

2
flight/pios/stm32f10x/pios_tim.c
View File

@ -134,7 +134,7 @@ int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel *
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
break;
case (uint32_t) GPIOC:
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
break;
default:
PIOS_Assert(0);

4
flight/pios/stm32f10x/pios_usart.c
View File

@ -196,7 +196,7 @@ out_fail:
return(-1);
}
static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
@ -205,7 +205,7 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
}
static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail)
static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;

2
flight/pios/stm32f10x/pios_usb.c
View File

@ -208,7 +208,7 @@ int32_t PIOS_USB_Reenumerate()
return 0;
}
bool PIOS_USB_CableConnected(uint8_t id)
bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id;

2
flight/pios/stm32f10x/pios_usb_cdc.c
View File

@ -229,7 +229,7 @@ static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev)
#endif /* PIOS_INCLUDE_FREERTOS */
}
static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail)
static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;

2
flight/pios/stm32f10x/pios_usb_hid.c
View File

@ -223,7 +223,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) {
PIOS_IRQ_Enable();
}
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail)
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id;

2
flight/pios/stm32f10x/pios_usb_hid_pwr.c
View File

@ -44,7 +44,7 @@ struct {
* Output : None.
* Return : None
*******************************************************************************/
void USB_Cable_Config(FunctionalState NewState)
void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState)
{
}

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