rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'remotes/origin/sambas/diffnext' into next

Browse Source
This commit is contained in:
Sambas 2013-05-03 07:58:17 +03:00
commit 4d19fd5989
81 changed files with 6803 additions and 4832 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

263
flight/libraries/paths.c
View File

@ -32,8 +32,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 +46,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 +75,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 +117,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 +173,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 * M_PI;
}
if (a_radius < 0) {
a_radius += 2 * M_PI;
}
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
status->error = fabs(status->error);
if (progress < 0) {
progress += 1.0;
} else if (progress >= 1) {
progress -= 1.0;
}
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);
}

60
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;
@ -110,21 +111,56 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) {
// Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports VelocityControl
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol){
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports Position Hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports AutoLand Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports POI Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
@ -141,7 +177,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);
@ -158,14 +194,14 @@ int32_t configuration_check()
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)
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_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 +217,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;
}

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

@ -39,7 +39,9 @@
#include "openpilot.h"
#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
@ -55,12 +57,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);
@ -73,9 +78,11 @@ int32_t AltitudeStart()
{
if (altitudeEnabled) {
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeInitialize();
#endif
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialze();
SonarAltitudeInitialize();
#endif
// Start main task
@ -105,11 +112,13 @@ 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)
@ -118,46 +127,50 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
*/
static void altitudeTask(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
@ -196,6 +209,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);

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

@ -61,10 +61,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);
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
@ -79,7 +75,9 @@ int32_t AltitudeStart()
int32_t AltitudeInitialize()
{
BaroAltitudeInitialize();
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
@ -92,9 +90,8 @@ static void altitudeTask(void *parameters)
#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
@ -111,27 +108,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;

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

@ -587,8 +587,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);

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

@ -43,9 +43,9 @@
#include "magnetometer.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
@ -53,14 +53,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);
@ -75,7 +81,7 @@ int32_t MagBaroStart()
if (magbaroEnabled) {
// Start main task
xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
//TaskMonitorAdd(TASKINFO_RUNNING_MAGBARO, taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MAGBARO, taskHandle);
return 0;
}
return -1;
@ -102,13 +108,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;
}
@ -116,7 +127,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,
@ -127,27 +138,25 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
};
#endif
static void magbaroTask(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)
@ -160,7 +169,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
@ -170,7 +179,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;
@ -192,7 +201,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);

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 ( \

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

@ -73,11 +73,7 @@
// Private types
typedef enum
{
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
@ -93,6 +89,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);
@ -113,10 +110,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;
@ -130,12 +128,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);
TaskMonitorAdd(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);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
return 0;
}
/**
@ -144,548 +142,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)
{
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] == 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) {
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) {
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,55 +698,86 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
*/
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
lastSysTime = thisSysTime;
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
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_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);
*/
}
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(ManualControlCommandData * cmd, bool changed)
{
static portTickType lastSysTime;
portTickType thisSysTime;
float dT;
thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
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);
}
/**
@ -754,41 +787,43 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
*/
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{
const float DEADBAND_HIGH = 0.55;
const float DEADBAND_LOW = 0.45;
static portTickType lastSysTime;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
const float DEADBAND_HIGH = 0.55;
const float DEADBAND_LOW = 0.45;
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
static portTickType lastSysTime;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
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);
thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
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);
}
#else
@ -797,12 +832,17 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
// in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
#endif
/**
@ -810,36 +850,39 @@ 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.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
// Bound
if (valueScaled > 1.0) {
valueScaled = 1.0;
} else if (valueScaled < -1.0) {
valueScaled = -1.0;
}
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;
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;
}
/**
@ -848,24 +891,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;
// 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 false;
}
}
return true;
return true;
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
@ -873,28 +913,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);
}
}
/**
@ -905,116 +946,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
}
}
/**
@ -1025,20 +1071,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);
}
}
/**
@ -1047,13 +1093,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);
}
/**
@ -1061,13 +1106,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 (fabs(*value) < deadband)
*value = 0.0f;
else if (*value > 0.0f)
*value -= deadband;
else
*value += deadband;
}
#ifdef USE_INPUT_LPF
@ -1076,25 +1120,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)
{
configuration_check();
configuration_check();
}
/**
* @}
* @}
*/
* @}
* @}
*/

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,55 +67,38 @@ 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)
{
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;;
portTickType xDelay = 100 / 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;
}
}
// ****************
/**
* @}
* @}
*/
* @}
* @}
*/

45
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))
@ -123,7 +142,6 @@ struct FontDimensions
// 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_ */

3361
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
* @{
*
@ -29,7 +29,6 @@
*/
// ****************
#include "openpilot.h"
#include "osdgen.h"
#include "attitudeactual.h"
@ -39,6 +38,7 @@
#include "gpssatellites.h"
#include "osdsettings.h"
#include "baroaltitude.h"
#include "flightstatus.h"
#include "fonts.h"
#include "font12x18.h"
@ -47,29 +47,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;
// ****************
@ -94,143 +93,136 @@ 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] = {
{ oplogo_width,
oplogo_height,
oplogo_bits,
oplogo_mask_bits },
{ level_width,
level_height,
level_bits,
level_mask_bits },
{ llama_width,
llama_height,
llama_bits,
llama_mask_bits },
struct splashEntry splash[3] =
{
{ oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits },
{ level_width, level_height, level_bits, level_mask_bits },
{ llama_width, llama_height, llama_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;
}
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 };
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.
@ -247,10 +239,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
@ -275,62 +267,58 @@ 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.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
error = (int64_t)(
doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + 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
@ -347,14 +335,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);
}
/**
@ -368,18 +356,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
*
@ -391,41 +378,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);
}
}
}
/**
@ -439,10 +423,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);
}
/**
@ -459,19 +443,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);
}
/**
@ -485,27 +468,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. */
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);
}
}
/**
@ -519,10 +502,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);
}
/**
@ -539,19 +522,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);
}
/**
@ -570,61 +552,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;
}
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;
}
} 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++;
}
}
}
/**
@ -639,8 +616,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);
}
/**
@ -656,14 +633,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);
}
/**
@ -679,22 +656,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;
}
}
}
/**
@ -710,58 +684,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;
}
}
}
/**
@ -775,42 +742,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);
}
}
/**
@ -825,45 +787,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
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;
}
}
}
/**
@ -878,8 +834,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);
}
/**
@ -897,84 +853,70 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i
*/
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, 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;
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;
}
}
}
/**
@ -991,12 +933,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);
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);
}
}
/**
@ -1016,12 +959,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);
}
}
/**
@ -1041,12 +985,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);
}
}
@ -1064,8 +1009,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);
}
/**
@ -1076,22 +1021,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 (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
@ -1110,7 +1055,7 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
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);
@ -1119,58 +1064,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
@ -1195,98 +1137,98 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
// 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;
@ -1295,55 +1237,61 @@ 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)
*/
* 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)
{
int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
@ -1360,78 +1308,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;
@ -1439,204 +1385,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.
@ -1655,148 +1593,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, 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);
}
/**
@ -1815,599 +1739,608 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
*/
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)
{
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) && (angle != -90))
{
k = tanf(alpha);
vertical = 0;
if(k==0)
{
horizontal=1;
}
}
else
{
vertical = 1;
}
//
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) && (angle != -90)) {
k = tanf(alpha);
vertical = 0;
if (k == 0) {
horizontal = 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 && angle>-90)
{
//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)
{
//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 && angle<90)
{
//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)
{
//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)
{
//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 && angle > -90) {
//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) {
//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 && angle < 90) {
//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) {
//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) {
//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;
}
// yaw corrected bearing, needs compass
u2g=brng-180-m_yaw;
if(u2g<0)
u2g+=360;
u2g = brng - 180 - m_yaw;
if (u2g < 0) {
u2g += 360;
}
// 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));
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;
// Elevation v depends servo direction
if(d!=0)
elevation = 90-RAD2DEG(atanf(dAlt/d));
if (d != 0)
elevation = 90 - RAD2DEG(atanf(dAlt/d));
else
elevation = 0;
//! 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);
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(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", (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", (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(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
@ -2417,12 +2350,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);
//TaskMonitorAdd(TASKINFO_RUNNING_GPS, osdgenTaskHandle);
return 0;
// Start gps task
vSemaphoreCreateBinary(osdSemaphore);
xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN);
#endif
return 0;
}
/**
@ -2432,23 +2367,24 @@ 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)
// ****************
/**
@ -2457,44 +2393,51 @@ MODULE_INITCALL(osdgenInitialize, osdgenStart)
static void osdgenTask(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_ */

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

@ -29,264 +29,150 @@
*/
// ****************
#include "openpilot.h"
#include "osdinput.h"
#include "attitudeactual.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);
//TaskMonitorAdd(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(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(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)
/**
* @}
* @}
*/

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

@ -237,20 +237,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);

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;
}

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

@ -53,6 +53,9 @@
#include "relay_tuning.h"
#include "virtualflybar.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
// Private constants
#define MAX_QUEUE_SIZE 1
@ -123,7 +126,6 @@ int32_t StabilizationInitialize()
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
#endif
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
@ -211,7 +213,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];
@ -277,6 +284,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)
@ -292,6 +300,7 @@ static void stabilizationTask(void* parameters)
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
@ -307,7 +316,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);

598
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)
@ -111,15 +111,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
TaskMonitorAdd(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
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
return 0;
}
/**
@ -129,26 +129,26 @@ 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)
@ -163,76 +163,76 @@ static void systemTask(void *parameters)
/* 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);
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Main system loop
while (1) {
// 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
// Update the task status object
TaskMonitorUpdateAll();
// Update the task status object
TaskMonitorUpdateAll();
#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);
}
}
}
/**
@ -240,102 +240,95 @@ 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;
}
}
}
/**
@ -355,76 +348,75 @@ static void hwSettingsUpdatedCb(UAVObjEvent * ev)
* 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;
}
/**
@ -432,43 +424,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.43; /* V */
const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
#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 / STM32_TEMP_AVG_SLOPE + 25;
#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 / STM32_TEMP_AVG_SLOPE + 25;
#endif
SystemStatsSet(&stats);
#endif
SystemStatsSet(&stats);
}
/**
@ -476,66 +474,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);
}
}
/**
@ -543,13 +539,13 @@ 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;
}
}
/**
@ -558,11 +554,11 @@ void vApplicationIdleHook(void)
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, 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
}
@ -572,15 +568,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
}
/**
* @}
* @}
*/
* @}
* @}
*/

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

@ -71,6 +71,11 @@
#include "velocityactual.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
@ -80,19 +85,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
@ -100,13 +108,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);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
if (vtolpathfollower_enabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *) "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
return 0;
}
/**
@ -115,25 +123,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,116 +161,203 @@ static float throttleOffset = 0;
*/
static void vtolPathFollowerTask(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.10;
const float DEADBAND_LOW = -0.10;
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;
} else {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
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 (pathAngle != 0 || changeRadius != 0) {
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;
pathDesired.EndingVelocity = 0;
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);
}
}
/**
@ -269,51 +368,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);
}
/**
@ -324,38 +446,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};
@ -363,50 +488,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);
}
/**
@ -415,16 +536,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);
}
/**
@ -434,128 +555,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);
}
/**
@ -563,39 +684,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);
}
/**
@ -603,17 +724,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)
{
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);
}
}
}
}

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 */

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 */
/**
* @}
* @}
*/

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

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

919
flight/pios/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c
View File

@ -1,552 +1,553 @@
/**
******************************************************************************
* @file system_stm32f4xx.c
* @author MCD Application Team
* @version V1.0.1
* @date 24-January-2012
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F4xx devices,
* and is generated by the clock configuration tool
* stm32f4xx_Clock_Configuration_V1.0.1.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define
* in "stm32f4xx.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
*=============================================================================
* Supported STM32F4xx device revision | Rev A
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 108000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 108000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 4
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*-----------------------------------------------------------------------------
* PLL_M | 4
*-----------------------------------------------------------------------------
* PLL_N | 216
*-----------------------------------------------------------------------------
* PLL_P | 4
*-----------------------------------------------------------------------------
* PLL_Q | 9
*-----------------------------------------------------------------------------
* PLLI2S_N | NA
*-----------------------------------------------------------------------------
* PLLI2S_R | NA
*-----------------------------------------------------------------------------
* I2S input clock | NA
*-----------------------------------------------------------------------------
* VDD(V) | 3.3
*-----------------------------------------------------------------------------
* Main regulator output voltage | Scale2 mode
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 3
*-----------------------------------------------------------------------------
* Prefetch Buffer | OFF
*-----------------------------------------------------------------------------
* Instruction cache | ON
*-----------------------------------------------------------------------------
* Data cache | ON
*-----------------------------------------------------------------------------
* Require 48MHz for USB OTG FS, | Disabled
* SDIO and RNG clock |
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
******************************************************************************
* @file system_stm32f4xx.c
* @author MCD Application Team
* @version V1.0.0
* @date 30-September-2011
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F4xx devices,
* and is generated by the clock configuration tool
* stm32f4xx_Clock_Configuration_V1.0.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define
* in "stm32f4xx.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
*=============================================================================
* Supported STM32F4xx device revision | Rev A
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 4
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*-----------------------------------------------------------------------------
* PLL_M | 10
*-----------------------------------------------------------------------------
* PLL_N | 420
*-----------------------------------------------------------------------------
* PLL_P | 2
*-----------------------------------------------------------------------------
* PLL_Q | 7
*-----------------------------------------------------------------------------
* PLLI2S_N | NA
*-----------------------------------------------------------------------------
* PLLI2S_R | NA
*-----------------------------------------------------------------------------
* I2S input clock | NA
*-----------------------------------------------------------------------------
* VDD(V) | 3.3
*-----------------------------------------------------------------------------
* Main regulator output voltage | Scale1 mode
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 5
*-----------------------------------------------------------------------------
* Prefetch Buffer | OFF
*-----------------------------------------------------------------------------
* Instruction cache | ON
*-----------------------------------------------------------------------------
* Data cache | ON
*-----------------------------------------------------------------------------
* Require 48MHz for USB OTG FS, | Enabled
* SDIO and RNG clock |
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
* @{
*/
/** @addtogroup stm32f4xx_system
* @{
*/
* @{
*/
/** @addtogroup STM32F4xx_System_Private_Includes
* @{
*/
* @{
*/
#include "stm32f4xx.h"
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
* @{
*/
* @{
*/
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Defines
* @{
*/
* @{
*/
/************************* Miscellaneous Configuration ************************/
/*!< Uncomment the following line if you need to use external SRAM mounted
on STM324xG_EVAL board as data memory */
on STM324xG_EVAL board as data memory */
/* #define DATA_IN_ExtSRAM */
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
This value must be a multiple of 0x200. */
/******************************************************************************/
/************************* PLL Parameters *************************************/
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
#define PLL_M 4
#define PLL_N 216
#define PLL_M 10
#define PLL_N 420
/* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 4
#define PLL_P 2
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 9
#define PLL_Q 7
/******************************************************************************/
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Macros
* @{
*/
* @{
*/
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Variables
* @{
*/
* @{
*/
uint32_t SystemCoreClock = 108000000;
uint32_t SystemCoreClock = 168000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
* @{
*/
* @{
*/
static void SetSysClock(void);
#ifdef DATA_IN_ExtSRAM
static void SystemInit_ExtMemCtl(void);
static void SystemInit_ExtMemCtl(void);
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Functions
* @{
*/
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR = 0x00000000;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset PLLCFGR register */
RCC->PLLCFGR = 0x24003010;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR = 0x00000000;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset PLLCFGR register */
RCC->PLLCFGR = 0x24003010;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
#ifdef DATA_IN_ExtSRAM
SystemInit_ExtMemCtl();
SystemInit_ExtMemCtl();
#endif /* DATA_IN_ExtSRAM */
/* Configure the System clock source, PLL Multiplier and Divider factors,
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
SetSysClock();
/* Configure the Vector Table location add offset address ------------------*/
SetSysClock();
/* Configure the Vector Table location add offset address ------------------*/
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value
* 25 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value
* 25 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
SYSCLK = PLL_VCO / PLL_P
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
else
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp;
break;
default:
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK frequency --------------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK frequency */
SystemCoreClock >>= tmp;
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
SYSCLK = PLL_VCO / PLL_P
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
else
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp;
break;
default:
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK frequency --------------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @Note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @Note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
static void SetSysClock(void)
{
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Select regulator voltage output Scale 2 mode, System frequency up to 144 MHz */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR &= (uint32_t)~(PWR_CR_VOS);
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_3WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR |= PWR_CR_VOS;
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @brief Setup the external memory controller. Called in startup_stm32f4xx.s
* before jump to __main
* @param None
* @retval None
*/
* @brief Setup the external memory controller. Called in startup_stm32f4xx.s
* before jump to __main
* @param None
* @retval None
*/
#ifdef DATA_IN_ExtSRAM
/**
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external SRAM mounted on STM324xG_EVAL board
* This SRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external SRAM mounted on STM324xG_EVAL board
* This SRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
void SystemInit_ExtMemCtl(void)
{
/*-- GPIOs Configuration -----------------------------------------------------*/
/*
+-------------------+--------------------+------------------+------------------+
+ SRAM pins assignment +
+-------------------+--------------------+------------------+------------------+
| PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 |
| PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 |
| PD4 <-> FSMC_NOE | PE3 <-> FSMC_A19 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 |
| PD5 <-> FSMC_NWE | PE4 <-> FSMC_A20 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 |
| PD8 <-> FSMC_D13 | PE7 <-> FSMC_D4 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 |
| PD9 <-> FSMC_D14 | PE8 <-> FSMC_D5 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 |
| PD10 <-> FSMC_D15 | PE9 <-> FSMC_D6 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 |
| PD11 <-> FSMC_A16 | PE10 <-> FSMC_D7 | PF13 <-> FSMC_A7 |------------------+
| PD12 <-> FSMC_A17 | PE11 <-> FSMC_D8 | PF14 <-> FSMC_A8 |
| PD13 <-> FSMC_A18 | PE12 <-> FSMC_D9 | PF15 <-> FSMC_A9 |
| PD14 <-> FSMC_D0 | PE13 <-> FSMC_D10 |------------------+
| PD15 <-> FSMC_D1 | PE14 <-> FSMC_D11 |
| | PE15 <-> FSMC_D12 |
+-------------------+--------------------+
*/
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
RCC->AHB1ENR = 0x00000078;
/* Connect PDx pins to FSMC Alternate function */
GPIOD->AFR[0] = 0x00cc00cc;
GPIOD->AFR[1] = 0xcc0ccccc;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xaaaa0a0a;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xffff0f0f;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FSMC Alternate function */
GPIOE->AFR[0] = 0xc00cc0cc;
GPIOE->AFR[1] = 0xcccccccc;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xaaaa828a;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xffffc3cf;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FSMC Alternate function */
GPIOF->AFR[0] = 0x00cccccc;
GPIOF->AFR[1] = 0xcccc0000;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xaa000aaa;
/* Configure PFx pins speed to 100 MHz */
GPIOF->OSPEEDR = 0xff000fff;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FSMC Alternate function */
GPIOG->AFR[0] = 0x00cccccc;
GPIOG->AFR[1] = 0x000000c0;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0x00080aaa;
/* Configure PGx pins speed to 100 MHz */
GPIOG->OSPEEDR = 0x000c0fff;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/*-- FSMC Configuration ------------------------------------------------------*/
/* Enable the FSMC interface clock */
RCC->AHB3ENR = 0x00000001;
/* Configure and enable Bank1_SRAM2 */
FSMC_Bank1->BTCR[2] = 0x00001015;
FSMC_Bank1->BTCR[3] = 0x00010603;
FSMC_Bank1E->BWTR[2] = 0x0fffffff;
/*
Bank1_SRAM2 is configured as follow:
p.FSMC_AddressSetupTime = 3;
p.FSMC_AddressHoldTime = 0;
p.FSMC_DataSetupTime = 6;
p.FSMC_BusTurnAroundDuration = 1;
p.FSMC_CLKDivision = 0;
p.FSMC_DataLatency = 0;
p.FSMC_AccessMode = FSMC_AccessMode_A;
FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_PSRAM;
FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
*/
/*-- GPIOs Configuration -----------------------------------------------------*/
/*
+-------------------+--------------------+------------------+------------------+
+ SRAM pins assignment +
+-------------------+--------------------+------------------+------------------+
| PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 |
| PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 |
| PD4 <-> FSMC_NOE | PE3 <-> FSMC_A19 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 |
| PD5 <-> FSMC_NWE | PE4 <-> FSMC_A20 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 |
| PD8 <-> FSMC_D13 | PE7 <-> FSMC_D4 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 |
| PD9 <-> FSMC_D14 | PE8 <-> FSMC_D5 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 |
| PD10 <-> FSMC_D15 | PE9 <-> FSMC_D6 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 |
| PD11 <-> FSMC_A16 | PE10 <-> FSMC_D7 | PF13 <-> FSMC_A7 |------------------+
| PD12 <-> FSMC_A17 | PE11 <-> FSMC_D8 | PF14 <-> FSMC_A8 |
| PD13 <-> FSMC_A18 | PE12 <-> FSMC_D9 | PF15 <-> FSMC_A9 |
| PD14 <-> FSMC_D0 | PE13 <-> FSMC_D10 |------------------+
| PD15 <-> FSMC_D1 | PE14 <-> FSMC_D11 |
| | PE15 <-> FSMC_D12 |
+-------------------+--------------------+
*/
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
RCC->AHB1ENR = 0x00000078;
/* Connect PDx pins to FSMC Alternate function */
GPIOD->AFR[0] = 0x00cc00cc;
GPIOD->AFR[1] = 0xcc0ccccc;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xaaaa0a0a;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xffff0f0f;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FSMC Alternate function */
GPIOE->AFR[0] = 0xc00cc0cc;
GPIOE->AFR[1] = 0xcccccccc;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xaaaa828a;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xffffc3cf;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FSMC Alternate function */
GPIOF->AFR[0] = 0x00cccccc;
GPIOF->AFR[1] = 0xcccc0000;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xaa000aaa;
/* Configure PFx pins speed to 100 MHz */
GPIOF->OSPEEDR = 0xff000fff;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FSMC Alternate function */
GPIOG->AFR[0] = 0x00cccccc;
GPIOG->AFR[1] = 0x000000c0;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0x00080aaa;
/* Configure PGx pins speed to 100 MHz */
GPIOG->OSPEEDR = 0x000c0fff;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/*-- FSMC Configuration ------------------------------------------------------*/
/* Enable the FSMC interface clock */
RCC->AHB3ENR = 0x00000001;
/* Configure and enable Bank1_SRAM2 */
FSMC_Bank1->BTCR[2] = 0x00001015;
FSMC_Bank1->BTCR[3] = 0x00010603;
FSMC_Bank1E->BWTR[2] = 0x0fffffff;
/*
Bank1_SRAM2 is configured as follow:
p.FSMC_AddressSetupTime = 3;
p.FSMC_AddressHoldTime = 0;
p.FSMC_DataSetupTime = 6;
p.FSMC_BusTurnAroundDuration = 1;
p.FSMC_CLKDivision = 0;
p.FSMC_DataLatency = 0;
p.FSMC_AccessMode = FSMC_AccessMode_A;
FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_PSRAM;
FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
*/
}
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
* @}
*/
/**
* @}
*/
* @}
*/
/**
* @}
*/
* @}
*/
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

400
flight/pios/stm32f4xx/pios_sys.c
View File

@ -43,184 +43,184 @@ void SysTick_Handler(void);
#define MEM32(addr) (*((volatile uint32_t *)(addr)))
/**
* Initialises all system peripherals
*/
* Initialises all system peripherals
*/
void PIOS_SYS_Init(void)
{
/* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */
SystemInit();
SystemCoreClockUpdate(); /* update SystemCoreClock for use elsewhere */
/* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */
SystemInit();
SystemCoreClockUpdate(); /* update SystemCoreClock for use elsewhere */
/*
* @todo might make sense to fetch the bus clocks and save them somewhere to avoid
* having to use the clunky get-all-clocks API everytime we need one.
*/
/*
* @todo might make sense to fetch the bus clocks and save them somewhere to avoid
* having to use the clunky get-all-clocks API everytime we need one.
*/
/* Initialise Basic NVIC */
/* do this early to ensure that we take exceptions in the right place */
NVIC_Configuration();
/* Initialise Basic NVIC */
/* do this early to ensure that we take exceptions in the right place */
NVIC_Configuration();
/* Init the delay system */
PIOS_DELAY_Init();
/* Init the delay system */
PIOS_DELAY_Init();
/*
* Turn on all the peripheral clocks.
* Micromanaging clocks makes no sense given the power situation in the system, so
* light up everything we might reasonably use here and just leave it on.
*/
RCC_AHB1PeriphClockCmd(
RCC_AHB1Periph_GPIOA |
RCC_AHB1Periph_GPIOB |
RCC_AHB1Periph_GPIOC |
RCC_AHB1Periph_GPIOD |
RCC_AHB1Periph_GPIOE |
RCC_AHB1Periph_GPIOF |
RCC_AHB1Periph_GPIOG |
RCC_AHB1Periph_GPIOH |
RCC_AHB1Periph_GPIOI |
RCC_AHB1Periph_CRC |
RCC_AHB1Periph_FLITF |
RCC_AHB1Periph_SRAM1 |
RCC_AHB1Periph_SRAM2 |
RCC_AHB1Periph_BKPSRAM |
RCC_AHB1Periph_DMA1 |
RCC_AHB1Periph_DMA2 |
//RCC_AHB1Periph_ETH_MAC | No ethernet
//RCC_AHB1Periph_ETH_MAC_Tx |
//RCC_AHB1Periph_ETH_MAC_Rx |
//RCC_AHB1Periph_ETH_MAC_PTP |
//RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY)
//RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above)
0, ENABLE);
RCC_AHB2PeriphClockCmd(
//RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support?
//RCC_AHB2Periph_CRYP | No crypto
//RCC_AHB2Periph_HASH | No hash generator
//RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired
//RCC_AHB2Periph_OTG_FS |
0, ENABLE);
RCC_AHB3PeriphClockCmd(
//RCC_AHB3Periph_FSMC | No external static memory
0, ENABLE);
RCC_APB1PeriphClockCmd(
RCC_APB1Periph_TIM2 |
RCC_APB1Periph_TIM3 |
RCC_APB1Periph_TIM4 |
RCC_APB1Periph_TIM5 |
RCC_APB1Periph_TIM6 |
RCC_APB1Periph_TIM7 |
RCC_APB1Periph_TIM12 |
RCC_APB1Periph_TIM13 |
RCC_APB1Periph_TIM14 |
RCC_APB1Periph_WWDG |
RCC_APB1Periph_SPI2 |
RCC_APB1Periph_SPI3 |
RCC_APB1Periph_USART2 |
RCC_APB1Periph_USART3 |
RCC_APB1Periph_UART4 |
RCC_APB1Periph_UART5 |
RCC_APB1Periph_I2C1 |
RCC_APB1Periph_I2C2 |
RCC_APB1Periph_I2C3 |
RCC_APB1Periph_CAN1 |
RCC_APB1Periph_CAN2 |
RCC_APB1Periph_PWR |
RCC_APB1Periph_DAC |
0, ENABLE);
/*
* Turn on all the peripheral clocks.
* Micromanaging clocks makes no sense given the power situation in the system, so
* light up everything we might reasonably use here and just leave it on.
*/
RCC_AHB1PeriphClockCmd(
RCC_AHB1Periph_GPIOA |
RCC_AHB1Periph_GPIOB |
RCC_AHB1Periph_GPIOC |
RCC_AHB1Periph_GPIOD |
RCC_AHB1Periph_GPIOE |
RCC_AHB1Periph_GPIOF |
RCC_AHB1Periph_GPIOG |
RCC_AHB1Periph_GPIOH |
RCC_AHB1Periph_GPIOI |
RCC_AHB1Periph_CRC |
RCC_AHB1Periph_FLITF |
RCC_AHB1Periph_SRAM1 |
RCC_AHB1Periph_SRAM2 |
RCC_AHB1Periph_BKPSRAM |
RCC_AHB1Periph_DMA1 |
RCC_AHB1Periph_DMA2 |
//RCC_AHB1Periph_ETH_MAC | No ethernet
//RCC_AHB1Periph_ETH_MAC_Tx |
//RCC_AHB1Periph_ETH_MAC_Rx |
//RCC_AHB1Periph_ETH_MAC_PTP |
//RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY)
//RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above)
0, ENABLE);
RCC_AHB2PeriphClockCmd(
//RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support?
//RCC_AHB2Periph_CRYP | No crypto
//RCC_AHB2Periph_HASH | No hash generator
//RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired
//RCC_AHB2Periph_OTG_FS |
0, ENABLE);
RCC_AHB3PeriphClockCmd(
//RCC_AHB3Periph_FSMC | No external static memory
0, ENABLE);
RCC_APB1PeriphClockCmd(
RCC_APB1Periph_TIM2 |
RCC_APB1Periph_TIM3 |
RCC_APB1Periph_TIM4 |
RCC_APB1Periph_TIM5 |
RCC_APB1Periph_TIM6 |
RCC_APB1Periph_TIM7 |
RCC_APB1Periph_TIM12 |
RCC_APB1Periph_TIM13 |
RCC_APB1Periph_TIM14 |
RCC_APB1Periph_WWDG |
RCC_APB1Periph_SPI2 |
RCC_APB1Periph_SPI3 |
RCC_APB1Periph_USART2 |
RCC_APB1Periph_USART3 |
RCC_APB1Periph_UART4 |
RCC_APB1Periph_UART5 |
RCC_APB1Periph_I2C1 |
RCC_APB1Periph_I2C2 |
RCC_APB1Periph_I2C3 |
RCC_APB1Periph_CAN1 |
RCC_APB1Periph_CAN2 |
RCC_APB1Periph_PWR |
RCC_APB1Periph_DAC |
0, ENABLE);
RCC_APB2PeriphClockCmd(
RCC_APB2Periph_TIM1 |
RCC_APB2Periph_TIM8 |
RCC_APB2Periph_USART1 |
RCC_APB2Periph_USART6 |
RCC_APB2Periph_ADC |
RCC_APB2Periph_ADC1 |
RCC_APB2Periph_ADC2 |
RCC_APB2Periph_ADC3 |
RCC_APB2Periph_SDIO |
RCC_APB2Periph_SPI1 |
RCC_APB2Periph_SYSCFG |
RCC_APB2Periph_TIM9 |
RCC_APB2Periph_TIM10 |
RCC_APB2Periph_TIM11 |
0, ENABLE);
RCC_APB2PeriphClockCmd(
RCC_APB2Periph_TIM1 |
RCC_APB2Periph_TIM8 |
RCC_APB2Periph_USART1 |
RCC_APB2Periph_USART6 |
RCC_APB2Periph_ADC |
RCC_APB2Periph_ADC1 |
RCC_APB2Periph_ADC2 |
RCC_APB2Periph_ADC3 |
RCC_APB2Periph_SDIO |
RCC_APB2Periph_SPI1 |
RCC_APB2Periph_SYSCFG |
RCC_APB2Periph_TIM9 |
RCC_APB2Periph_TIM10 |
RCC_APB2Periph_TIM11 |
0, ENABLE);
/*
* Configure all pins as input / pullup to avoid issues with
* uncommitted pins, excepting special-function pins that we need to
* remain as-is.
*/
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input
/*
* Configure all pins as input / pullup to avoid issues with
* uncommitted pins, excepting special-function pins that we need to
* remain as-is.
*/
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
#if (PIOS_USB_ENABLED)
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone
#endif
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4); // leave JTAG pins alone
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);// leave JTAG pins alone
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_Init(GPIOF, &GPIO_InitStructure);
GPIO_Init(GPIOG, &GPIO_InitStructure);
GPIO_Init(GPIOH, &GPIO_InitStructure);
GPIO_Init(GPIOI, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_Init(GPIOF, &GPIO_InitStructure);
GPIO_Init(GPIOG, &GPIO_InitStructure);
GPIO_Init(GPIOH, &GPIO_InitStructure);
GPIO_Init(GPIOI, &GPIO_InitStructure);
}
/**
* Shutdown PIOS and reset the microcontroller:<BR>
* <UL>
* <LI>Disable all RTOS tasks
* <LI>Disable all interrupts
* <LI>Turn off all board LEDs
* <LI>Reset STM32
* </UL>
* \return < 0 if reset failed
*/
* Shutdown PIOS and reset the microcontroller:<BR>
* <UL>
* <LI>Disable all RTOS tasks
* <LI>Disable all interrupts
* <LI>Turn off all board LEDs
* <LI>Reset STM32
* </UL>
* \return < 0 if reset failed
*/
int32_t PIOS_SYS_Reset(void)
{
/* Disable all RTOS tasks */
/* Disable all RTOS tasks */
#if defined(PIOS_INCLUDE_FREERTOS)
/* port specific FreeRTOS function to disable tasks (nested) */
portENTER_CRITICAL();
/* port specific FreeRTOS function to disable tasks (nested) */
portENTER_CRITICAL();
#endif
// disable all interrupts
PIOS_IRQ_Disable();
// disable all interrupts
PIOS_IRQ_Disable();
// turn off all board LEDs
// turn off all board LEDs
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
#if defined(PIOS_LED_ALARM)
PIOS_LED_Off(PIOS_LED_ALARM);
PIOS_LED_Off(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
/* XXX F10x port resets most (but not all) peripherals ... do we care? */
/* XXX F10x port resets most (but not all) peripherals ... do we care? */
/* Reset STM32 */
NVIC_SystemReset();
/* Reset STM32 */
NVIC_SystemReset();
while (1) ;
while (1);
/* We will never reach this point */
return -1;
/* We will never reach this point */
return -1;
}
/**
* Returns the CPU's flash size (in bytes)
*/
* Returns the CPU's flash size (in bytes)
*/
uint32_t PIOS_SYS_getCPUFlashSize(void)
{
return ((uint32_t) MEM16(0x1fff7a22) * 1024); // it might be possible to locate in the OTP area, but haven't looked and not documented
return ((uint32_t) MEM16(0x1fff7a22) * 1024); // it might be possible to locate in the OTP area, but haven't looked and not documented
}
/**
@ -231,97 +231,97 @@ uint32_t PIOS_SYS_getCPUFlashSize(void)
*/
int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array)
{
int i;
/* Stored in the so called "electronic signature" */
for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) {
uint8_t b = MEM8(0x1fff7a10 + i);
array[i] = b;
}
/* No error */
return 0;
int i;
/* Stored in the so called "electronic signature" */
for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) {
uint8_t b = MEM8(0x1fff7a10 + i);
array[i] = b;
}
/* No error */
return 0;
}
/**
* Returns the serial number as a string
* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
* (24 digits returned for STM32)
* return < 0 if feature not supported
*/
* Returns the serial number as a string
* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
* (24 digits returned for STM32)
* return < 0 if feature not supported
*/
int32_t PIOS_SYS_SerialNumberGet(char *str)
{
int i;
int i;
/* Stored in the so called "electronic signature" */
for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) {
uint8_t b = MEM8(0x1fff7a10 + (i / 2));
if (!(i & 1))
b >>= 4;
b &= 0x0f;
/* Stored in the so called "electronic signature" */
for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) {
uint8_t b = MEM8(0x1fff7a10 + (i / 2));
if (!(i & 1))
b >>= 4;
b &= 0x0f;
str[i] = ((b > 9) ? ('A' - 10) : '0') + b;
}
str[i] = '\0';
str[i] = ((b > 9) ? ('A' - 10) : '0') + b;
}
str[i] = '\0';
/* No error */
return 0;
/* No error */
return 0;
}
/**
* Configures Vector Table base location and SysTick
*/
* Configures Vector Table base location and SysTick
*/
void NVIC_Configuration(void)
{
/* Set the Vector Table base address as specified in .ld file */
extern void *pios_isr_vector_table_base;
NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0);
/* Set the Vector Table base address as specified in .ld file */
extern void *pios_isr_vector_table_base;
NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0);
/* 4 bits for Interrupt priorities so no sub priorities */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
/* 4 bits for Interrupt priorities so no sub priorities */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
/* Configure HCLK clock as SysTick clock source. */
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
/* Configure HCLK clock as SysTick clock source. */
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
}
#ifdef USE_FULL_ASSERT
/**
* Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* \param[in] file pointer to the source file name
* \param[in] line assert_param error line source number
* \retval None
*/
* Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* \param[in] file pointer to the source file name
* \param[in] line assert_param error line source number
* \retval None
*/
void assert_failed(uint8_t * file, uint32_t line)
{
/* When serial debugging is implemented, use something like this. */
/* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */
/* When serial debugging is implemented, use something like this. */
/* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */
/* Setup the LEDs to Alternate */
/* Setup the LEDs to Alternate */
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_On(PIOS_LED_HEARTBEAT);
PIOS_LED_On(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
#if defined(PIOS_LED_ALARM)
PIOS_LED_Off(PIOS_LED_ALARM);
PIOS_LED_Off(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
/* Infinite loop */
while (1) {
/* Infinite loop */
while (1) {
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
#if defined(PIOS_LED_ALARM)
PIOS_LED_Toggle(PIOS_LED_ALARM);
PIOS_LED_Toggle(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
for (int i = 0; i < 1000000; i++) ;
}
for (int i = 0; i < 1000000; i++);
}
}
#endif
#endif /* PIOS_INCLUDE_SYS */
/**
* @}
* @}
*/
* @}
* @}
*/

120
flight/targets/boards/coptercontrol/board_hw_defs.c
View File

@ -1067,6 +1067,82 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
#endif /* PIOS_INCLUDE_SBUS */
/*
* HK OSD
*/
static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
@ -1208,6 +1284,50 @@ const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = {
#endif
/*
* SONAR Inputs
*/
#if defined(PIOS_INCLUDE_HCSR04)
#include <pios_hcsr04_priv.h>
static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
{
.timer = TIM3,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
};
const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_hcsr04_port_all_channels,
.num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
.trigger = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
};
#endif
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -43,6 +43,7 @@ OPTMODULES += CameraStab
OPTMODULES += ComUsbBridge
OPTMODULES += GPS
OPTMODULES += TxPID
OPTMODULES += Osd/osdoutput
#OPTMODULES += Altitude
#OPTMODULES += Fault

41
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -59,6 +59,8 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_BRIDGE_RX_BUF_LEN 65
#define PIOS_COM_BRIDGE_TX_BUF_LEN 12
#define PIOS_COM_HKOSD_TX_BUF_LEN 22
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
uint32_t pios_com_debug_id;
@ -69,6 +71,7 @@ uint32_t pios_com_telem_usb_id;
uint32_t pios_com_vcp_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_bridge_id;
uint32_t pios_com_hkosd_id;
uint32_t pios_usb_rctx_id;
@ -546,6 +549,22 @@ void PIOS_Board_Init(void) {
}
}
break;
case HWSETTINGS_CC_MAINPORT_OSDHK:
{
uint32_t pios_usart_hkosd_id;
if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) {
PIOS_Assert(0);
}
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
NULL, 0,
tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
break;
}
/* Configure the flexi port */
@ -695,6 +714,22 @@ void PIOS_Board_Init(void) {
}
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_CC_FLEXIPORT_OSDHK:
{
uint32_t pios_usart_hkosd_id;
if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) {
PIOS_Assert(0);
}
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
NULL, 0,
tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
break;
}
/* Configure the rcvr port */
@ -703,6 +738,12 @@ void PIOS_Board_Init(void) {
switch (hwsettings_rcvrport) {
case HWSETTINGS_CC_RCVRPORT_DISABLED:
#if defined(PIOS_INCLUDE_HCSR04)
{
uint32_t pios_hcsr04_id;
PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg);
}
#endif
break;
case HWSETTINGS_CC_RCVRPORT_PWM:
#if defined(PIOS_INCLUDE_PWM)

25
flight/targets/boards/coptercontrol/pios_board.h
View File

@ -1,4 +1,4 @@
/**
/**
******************************************************************************
*
* @file pios_board.h
@ -30,14 +30,14 @@
// Timers and Channels Used
//------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | Servo 4 | | |
TIM2 | RC In 5 | RC In 6 | Servo 6 |
TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4
TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
------+-----------+-----------+-----------+----------
*/
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | Servo 4 | | |
TIM2 | RC In 5 | RC In 6 | Servo 6 |
TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4
TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
------+-----------+-----------+-----------+----------
*/
//------------------------
// DMA Channels Used
@ -55,7 +55,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
/* Channel 11 - */
/* Channel 12 - */
//------------------------
// BOOTLOADER_SETTINGS
//------------------------
@ -63,7 +62,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define BOARD_WRITABLE TRUE
#define MAX_DEL_RETRYS 3
//------------------------
// WATCHDOG_SETTINGS
//------------------------
@ -98,7 +96,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------
// PIOS_I2C
// See also pios_board.c
@ -155,6 +152,9 @@ extern uint32_t pios_com_debug_id;
#define PIOS_COM_DEBUG (pios_com_debug_id)
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_OSDHK (pios_com_hkosd_id)
//-------------------------
// ADC
// PIOS_ADC_PinGet(0) = Gyro Z
@ -264,7 +264,6 @@ extern uint32_t pios_com_debug_id;
#define PIOS_GPIO_CLKS { }
#define PIOS_GPIO_NUM 0
//-------------------------
// USB
//-------------------------

27
flight/targets/boards/osd/board-info.mk
View File

@ -13,16 +13,37 @@ OPENOCD_JTAG_CONFIG := stlink-v2.cfg
OPENOCD_CONFIG := stm32f4xx.stlink.cfg
#OPENOCD_CONFIG := stm32f4xx.cfg
# Flash memory map for OSD:
# Sector start size use
# 0 0x0800 0000 16k BL
# 1 0x0800 4000 16k BL
# 2 0x0800 8000 16k EE
# 3 0x0800 C000 16k EE
# 4 0x0801 0000 64k Unused
# 5 0x0802 0000 128k FW
# 6 0x0804 0000 128k FW
# 7 0x0806 0000 128k FW
# 8 0x0808 0000 128k Unused
# .. ..
# 11 0x080E 0000 128k Unused
# Note: These must match the values in link_$(BOARD)_memory.ld
BL_BANK_BASE := 0x08000000 # Start of bootloader flash
BL_BANK_SIZE := 0x00008000 # Should include BD_INFO region
# Leave the remaining 16KB and 64KB sectors for other uses
# Leave the remaining 16KB for settings storage
EE_BANK_BASE := 0x08008000 # EEPROM storage area
EE_BANK_SIZE := 0x00008000 # Size of EEPROM storage area
# Leave the reamaining 64KB sectors for other uses
FW_BANK_BASE := 0x08020000 # Start of firmware flash
FW_BANK_SIZE := 0x00040000 # Should include FW_DESC_SIZE
FW_BANK_SIZE := 0x00060000 # Should include FW_DESC_SIZE
FW_DESC_SIZE := 0x00000064
OSCILLATOR_FREQ := 8000000
SYSCLK_FREQ := 108000000
SYSCLK_FREQ := 168000000

219
flight/targets/boards/osd/board_hw_defs.c
View File

@ -33,9 +33,9 @@
static const struct pios_led pios_leds[] = {
[PIOS_LED_HEARTBEAT] = {
.pin = {
.gpio = GPIOC,
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
@ -47,7 +47,7 @@ static const struct pios_led pios_leds[] = {
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
@ -69,6 +69,30 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revisio
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_FLASH)
#include "pios_flashfs_logfs_priv.h"
#include "pios_flash_internal_priv.h"
static const struct pios_flash_internal_cfg flash_internal_cfg = {
};
static const struct flashfs_logfs_cfg flashfs_internal_cfg = {
.fs_magic = 0x99abcfef,
.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */
.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = EE_BANK_BASE, /* start after the bootloader */
.sector_size = 0x00004000, /* 16K bytes */
.page_size = 0x00004000, /* 16K bytes */
};
#include "pios_flash.h"
#endif /* PIOS_INCLUDE_FLASH */
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
@ -200,6 +224,7 @@ void PIOS_SPI_sdcard_irq_handler(void)
#include <pios_usart_priv.h>
#if defined(PIOS_INCLUDE_GPS)
/*
* GPS USART
*/
@ -244,7 +269,6 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = {
},
},
};
#endif /* PIOS_INCLUDE_GPS */
#ifdef PIOS_INCLUDE_COM_AUX
@ -519,14 +543,91 @@ const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
#if defined(PIOS_INCLUDE_VIDEO)
static const TIM_TimeBaseInitTypeDef tim_8_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = ((1000000 / 50000) - 1),
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_8_cfg = {
.timer = TIM8,
.time_base_init = &tim_8_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM8_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
/**
* Pios servo configuration structures
*/
#include <pios_servo_priv.h>
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
{
.timer = TIM8,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource8,
},
.remap = GPIO_AF_TIM8,
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource9,
},
.remap = GPIO_AF_TIM8,
},
};
const struct pios_servo_cfg pios_servo_cfg = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = 0,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
};
#include <pios_video.h>
static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = {
.vector = PIOS_Hsync_ISR,
.line = EXTI_Line2,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOD,
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
@ -535,28 +636,30 @@ static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = {
},
.irq = {
.init = {
.NVIC_IRQChannel = EXTI2_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannel = EXTI9_5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = EXTI_Line2, // matches above GPIO pin
.EXTI_Line = EXTI_Line7, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Rising_Falling,
//.EXTI_Trigger = EXTI_Trigger_Rising_Falling,
.EXTI_Trigger = EXTI_Trigger_Falling,
//.EXTI_Trigger = EXTI_Trigger_Rising,
.EXTI_LineCmd = ENABLE,
},
},
};
static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
.vector = PIOS_Vsync_ISR,
.line = EXTI_Line11,
.line = EXTI_Line5,
.pin = {
.gpio = GPIOC,
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
@ -565,7 +668,7 @@ static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
},
.irq = {
.init = {
.NVIC_IRQChannel = EXTI15_10_IRQn,
.NVIC_IRQChannel = EXTI9_5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
@ -573,7 +676,7 @@ static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
},
.exti = {
.init = {
.EXTI_Line = EXTI_Line11, // matches above GPIO pin
.EXTI_Line = EXTI_Line5, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Falling,
.EXTI_LineCmd = ENABLE,
@ -587,7 +690,7 @@ static const struct pios_video_cfg pios_video_cfg = {
.regs = SPI3,
.remap = GPIO_AF_SPI3,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_1Line_Tx,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
@ -595,7 +698,7 @@ static const struct pios_video_cfg pios_video_cfg = {
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_Low,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
},
.use_crc = false,
.dma = {
@ -604,12 +707,11 @@ static const struct pios_video_cfg pios_video_cfg = {
.flags = (DMA_IT_TCIF7),
.init = {
.NVIC_IRQChannel = DMA1_Stream7_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {},
.tx = {
.channel = DMA1_Stream7,
@ -624,9 +726,9 @@ static const struct pios_video_cfg pios_video_cfg = {
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_VeryHigh,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOMode = DMA_FIFOMode_Enable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_MemoryBurst = DMA_MemoryBurst_INC4,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
@ -651,16 +753,7 @@ static const struct pios_video_cfg pios_video_cfg = {
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.mosi = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.mosi = {},
.slave_count = 1,
},
.level = {
@ -683,12 +776,11 @@ static const struct pios_video_cfg pios_video_cfg = {
.flags = (DMA_IT_TCIF5),
.init = {
.NVIC_IRQChannel = DMA2_Stream5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {},
.tx = {
.channel = DMA2_Stream5,
@ -703,9 +795,9 @@ static const struct pios_video_cfg pios_video_cfg = {
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_VeryHigh,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOMode = DMA_FIFOMode_Enable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_MemoryBurst = DMA_MemoryBurst_INC4,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
@ -730,23 +822,56 @@ static const struct pios_video_cfg pios_video_cfg = {
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.mosi = {},
.slave_count = 1,
},
/////////////////
.hsync = &pios_exti_hsync_cfg,
.vsync = &pios_exti_vsync_cfg,
.pixel_timer = {
.timer = TIM4,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource6,
},
.remap = GPIO_AF_TIM4,
},
.hsync_capture = {
.timer = TIM4,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource7,
},
.remap = GPIO_AF_TIM4,
},
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = 1,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
};
#endif

6
flight/targets/boards/osd/firmware/Makefile
View File

@ -37,6 +37,9 @@ MODULES += Telemetry
OPTMODULES =
# Some diagnostics
CDEFS += -DDIAG_TASKS
# Misc options
CFLAGS += -ffast-math
@ -97,6 +100,9 @@ else
SRC += $(OPTESTS)/$(TESTAPP).c
endif
CDEFS += -DEE_BANK_BASE=$(EE_BANK_BASE)
CDEFS += -DEE_BANK_SIZE=$(EE_BANK_SIZE)
# Optional component libraries
include $(PIOS)/common/libraries/dosfs/library.mk

8
flight/targets/boards/osd/firmware/inc/pios_config.h
View File

@ -99,14 +99,16 @@
/* PIOS common peripherals */
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
/* #define PIOS_INCLUDE_SERVO */
#define PIOS_INCLUDE_SERVO
/* #define PIOS_INCLUDE_I2C_ESC */
/* #define PIOS_INCLUDE_OVERO */
/* #define PIOS_OVERO_SPI */
#define PIOS_INCLUDE_SDCARD
/* #define PIOS_USE_SETTINGS_ON_SDCARD */
#define LOG_FILENAME "startup.log"
/* #define PIOS_INCLUDE_FLASH */
/* #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS */
#define PIOS_INCLUDE_FLASH
#define PIOS_INCLUDE_FLASH_INTERNAL
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* #define FLASH_FREERTOS */
/* #define PIOS_INCLUDE_FLASH_EEPROM */

61
flight/targets/boards/osd/firmware/pios_board.c
View File

@ -85,8 +85,8 @@ void PIOS_ADC_DMC_irq_handler(void)
static void Clock(uint32_t spektrum_id);
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128
#define PIOS_COM_AUX_RX_BUF_LEN 512
#define PIOS_COM_AUX_TX_BUF_LEN 512
@ -104,7 +104,42 @@ uint32_t pios_com_gps_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_telem_rf_id;
/**
* TIM3 is triggered by the HSYNC signal into its ETR line and will divide the
* APB1_CLOCK to generate a pixel clock that is used by the SPI CLK lines.
* TIM4 will be synced to it and will divide by that times the pixel width to
* fire an IRQ when the last pixel of the line has been output. Then the timer will
* be rearmed and wait for the next HSYNC signal.
* The critical timing detail is that the task be _DISABLED_ at the end of the line
* before an extra pixel is clocked out
* or we will need to configure the DMA task per line
*/
#include "pios_tim_priv.h"
#define NTSC_PX_CLOCK 6797088
#define PAL_PX_CLOCK 6750130
#define PX_PERIOD ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1)
#define LINE_PERIOD PX_PERIOD * GRAPHICS_WIDTH
static const TIM_TimeBaseInitTypeDef tim_4_time_base = {
.TIM_Prescaler = 0, //PIOS_PERIPHERAL_APB1_CLOCK,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = LINE_PERIOD - 1,
.TIM_RepetitionCounter = 0x0000,
};
const static struct pios_tim_clock_cfg pios_tim4_cfg = {
.timer = TIM4,
.time_base_init = &tim_4_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
}
};
void PIOS_Board_Init(void) {
@ -119,17 +154,34 @@ void PIOS_Board_Init(void) {
PIOS_Assert(0);
}
#if defined(PIOS_INCLUDE_SDCARD)
/* Enable and mount the SDCard */
PIOS_SDCARD_Init(pios_spi_sdcard_id);
PIOS_SDCARD_MountFS(0);
#endif
#endif /* PIOS_INCLUDE_SPI */
#ifdef PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
uintptr_t flash_id;
uintptr_t fs_id;
PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg);
PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id);
#elif !defined(PIOS_USE_SETTINGS_ON_SDCARD)
#error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS)
#endif
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
HwSettingsInitialize();
#ifdef PIOS_INCLUDE_WDG
/* Initialize watchdog as early as possible to catch faults during init */
PIOS_WDG_Init();
#endif /* PIOS_INCLUDE_WDG */
/* Initialize the alarms library */
AlarmsInitialize();
@ -151,6 +203,7 @@ void PIOS_Board_Init(void) {
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
}
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
@ -406,6 +459,10 @@ void PIOS_Board_Init(void) {
#endif
#if defined(PIOS_INCLUDE_VIDEO)
PIOS_TIM_InitClock(&tim_8_cfg);
PIOS_Servo_Init(&pios_servo_cfg);
// Start the pixel and line clock counter
//PIOS_TIM_InitClock(&pios_tim4_cfg);
PIOS_Video_Init(&pios_video_cfg);
#endif
}

75
flight/targets/boards/osd/pios_board.h
View File

@ -1,4 +1,4 @@
/**
/**
******************************************************************************
*
* @file pios_board.h
@ -32,14 +32,14 @@
// Timers and Channels Used
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+------------+------------+------------+------------
TIM1 | DELAY |
TIM2 | | PPM Output | PPM Input |
TIM3 | TIMER INTERRUPT |
TIM4 | STOPWATCH |
------+------------+------------+------------+------------
*/
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+------------+------------+------------+------------
TIM1 | DELAY |
TIM2 | | PPM Output | PPM Input |
TIM3 | TIMER INTERRUPT |
TIM4 | STOPWATCH |
------+------------+------------+------------+------------
*/
//------------------------
// DMA Channels Used
@ -75,7 +75,7 @@ TIM4 | STOPWATCH |
//#define PIOS_PERIPHERAL_CLOCK
//#define PIOS_PERIPHERAL_CLOCK
#define PIOS_SYSCLK 108000000
#define PIOS_SYSCLK 168000000
// Peripherals that belongs to APB1 are:
// DAC |PWR |CAN1,2
// I2C1,2,3 |UART4,5 |USART3,2
@ -98,19 +98,12 @@ TIM4 | STOPWATCH |
//
#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK
//------------------------
// TELEMETRY
//------------------------
#define TELEM_QUEUE_SIZE 20
#define PIOS_TELEM_STACK_SIZE 624
// *****************************************************************
// System Settings
#define PIOS_MASTER_CLOCK 108000000ul
#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2)
// *****************************************************************
// Interrupt Priorities
@ -119,7 +112,6 @@ TIM4 | STOPWATCH |
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------
// WATCHDOG_SETTINGS
//------------------------
@ -129,7 +121,7 @@ TIM4 | STOPWATCH |
#define PIOS_WDG_STABILIZATION 0x0002
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_OSDGEN 0x0010
// *****************************************************************
// PIOS_LED
@ -148,13 +140,13 @@ TIM4 | STOPWATCH |
// Timer interrupt
/*#define TIMER_INT_TIMER TIM3
#define TIMER_INT_FUNC TIM3_IRQHandler
#define TIMER_INT_PRIORITY 2
#define TIMER_INT_FUNC TIM3_IRQHandler
#define TIMER_INT_PRIORITY 2
// *****************************************************************
// Stop watch timer
// *****************************************************************
// Stop watch timer
#define STOPWATCH_TIMER TIM4*/
#define STOPWATCH_TIMER TIM4*/
//------------------------
// PIOS_SPI
@ -195,7 +187,6 @@ extern uint32_t pios_com_telem_usb_id;
//extern uint32_t pios_com_gps_id;
//#define PIOS_COM_GPS (pios_com_gps_id)
#if defined(PIOS_INCLUDE_USB_HID)
extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
@ -206,12 +197,13 @@ extern uint32_t pios_com_telem_usb_id;
//-------------------------
// ADC
// PIOS_ADC_PinGet(0) = External voltage
// PIOS_ADC_PinGet(1) = AUX1 (PX2IO external pressure port)
// PIOS_ADC_PinGet(2) = AUX2 (Current sensor, if available)
// PIOS_ADC_PinGet(3) = AUX3
// PIOS_ADC_PinGet(4) = VREF
// PIOS_ADC_PinGet(5) = Temperature sensor
// PIOS_ADC_PinGet(0) = Current
// PIOS_ADC_PinGet(1) = Voltage
// PIOS_ADC_PinGet(2) = Flight
// PIOS_ADC_PinGet(3) = Temperature sensor
// PIOS_ADC_PinGet(4) = Video
// PIOS_ADC_PinGet(5) = RSSI
// PIOS_ADC_PinGet(6) = VREF
//-------------------------
#define PIOS_DMA_PIN_CONFIG \
@ -219,10 +211,10 @@ extern uint32_t pios_com_telem_usb_id;
{GPIOC, GPIO_Pin_0, ADC_Channel_10}, \
{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \
{GPIOC, GPIO_Pin_2, ADC_Channel_12}, \
{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */\
{GPIOC, GPIO_Pin_3, ADC_Channel_13}, \
{GPIOA, GPIO_Pin_7, ADC_Channel_7}, \
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */\
{NULL, 0, ADC_Channel_TempSensor} /* Temperature sensor */\
{NULL, 0, ADC_Channel_Vrefint} /* Voltage reference */\
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
@ -231,21 +223,21 @@ extern uint32_t pios_com_telem_usb_id;
#define PIOS_ADC_NUM_CHANNELS 7
#define PIOS_ADC_MAX_OVERSAMPLING 10
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_USE_TEMP_SENSOR 1
// *****************************************************************
// USB
#if defined(PIOS_INCLUDE_USB_HID)
#define PIOS_USB_ENABLED 1
#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT
#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
#define PIOS_IRQ_USB_PRIORITY 8
#define PIOS_USB_RX_BUFFER_SIZE 512
#define PIOS_USB_TX_BUFFER_SIZE 512
#define PIOS_USB_ENABLED 1
#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT
#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
#define PIOS_IRQ_USB_PRIORITY 8
#define PIOS_USB_RX_BUFFER_SIZE 512
#define PIOS_USB_TX_BUFFER_SIZE 512
#endif
// *****************************************************************
//--------------------------
// Timer controller settings
@ -274,7 +266,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id;
//------------------------
#define PIOS_BMP085_OVERSAMPLING 3
/**
* glue macros for file IO
* STM32 uses DOSFS for file IO

85
flight/targets/boards/revolution/board_hw_defs.c
View File

@ -905,6 +905,91 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
#endif /* PIOS_INCLUDE_DSM */
/*
* HK OSD
*/
static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
.regs = USART1,
.remap = GPIO_AF_USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
.regs = USART3,
.remap = GPIO_AF_USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>

3
flight/targets/boards/revolution/firmware/Makefile
View File

@ -34,6 +34,7 @@ MODULES += Altitude/revolution
MODULES += Airspeed/revolution
MODULES += AltitudeHold
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl
MODULES += Actuator
MODULES += GPS
@ -44,8 +45,8 @@ MODULES += FirmwareIAP
MODULES += Radio
MODULES += PathPlanner
MODULES += FixedWingPathFollower
MODULES += Osd/osdoutout
MODULES += Telemetry
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
OPTMODULES =

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings

14
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -217,6 +217,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
#define PIOS_COM_HKOSD_RX_BUF_LEN 22
#define PIOS_COM_HKOSD_TX_BUF_LEN 22
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
uint32_t pios_com_debug_id;
@ -227,6 +230,7 @@ uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telem_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
#if defined(PIOS_INCLUDE_RFM22B)
uint32_t pios_rfm22b_id = 0;
#endif
@ -576,7 +580,9 @@ void PIOS_Board_Init(void) {
case HWSETTINGS_RM_MAINPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
case HWSETTINGS_RM_MAINPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rm_mainport */
if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) {
@ -639,7 +645,11 @@ void PIOS_Board_Init(void) {
case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
} /* hwsettings_rv_flexiport */
case HWSETTINGS_RM_FLEXIPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rm_flexiport */
/* Initalize the RFM22B radio COM device. */
#if defined(PIOS_INCLUDE_RFM22B)

31
flight/targets/boards/revolution/pios_board.h
View File

@ -35,18 +35,18 @@
// Timers and Channels Used
//------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
//------------------------
// DMA Channels Used
@ -71,7 +71,6 @@ TIM8 | | | |
#define BOARD_WRITABLE true
#define MAX_DEL_RETRYS 3
//------------------------
// PIOS_LED
//------------------------
@ -146,11 +145,13 @@ extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
extern uint32_t pios_com_vcp_id;
extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_OSDHK (pios_com_hkosd_id)
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
extern uint32_t pios_com_debug_id;
@ -220,7 +221,6 @@ extern uint32_t pios_packet_handler;
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------
// PIOS_RCVR
// See also pios_board.c
@ -292,7 +292,8 @@ extern uint32_t pios_packet_handler;
#define PIOS_ADC_NUM_CHANNELS 4
#define PIOS_ADC_MAX_OVERSAMPLING 2
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f
#define PIOS_ADC_USE_TEMP_SENSOR 1
//-------------------------
// USB

156
flight/targets/boards/revoproto/board_hw_defs.c
View File

@ -1010,7 +1010,7 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
static const struct pios_usart_cfg pios_usart_sbus_auxsbus_cfg = {
.regs = UART4,
.remap = GPIO_AF_UART4,
.remap = GPIO_AF_UART4,
.init = {
.USART_BaudRate = 100000,
.USART_WordLength = USART_WordLength_8b,
@ -1068,6 +1068,91 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
#endif /* PIOS_INCLUDE_SBUS */
/*
* HK OSD
*/
static const struct pios_usart_cfg pios_usart_hkosd_auxsbus_cfg = {
.regs = UART4,
.remap = GPIO_AF_UART4,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = UART4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = {
.regs = USART6,
.remap = GPIO_AF_USART6,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART6_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
@ -1353,7 +1438,15 @@ static const TIM_TimeBaseInitTypeDef tim_1_time_base = {
.TIM_RepetitionCounter = 0x0000,
};
// Set up timers that only have inputs on APB2
static const TIM_TimeBaseInitTypeDef tim_8_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF,
.TIM_RepetitionCounter = 0x0000,
};
// Set up timers that only have inputs on APB1
static const TIM_TimeBaseInitTypeDef tim_4_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
@ -1405,6 +1498,19 @@ static const struct pios_tim_clock_cfg tim_5_cfg = {
},
};
static const struct pios_tim_clock_cfg tim_8_cfg = {
.timer = TIM8,
.time_base_init = &tim_8_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM8_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_9_cfg = {
.timer = TIM9,
.time_base_init = &tim_9_10_11_time_base,
@ -1831,6 +1937,52 @@ static const struct pios_ppm_cfg pios_ppm_cfg = {
#include "pios_rcvr_priv.h"
#endif /* PIOS_INCLUDE_RCVR */
/*
* SONAR Inputs
*/
#if defined(PIOS_INCLUDE_HCSR04)
#include <pios_hcsr04_priv.h>
static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
{
.timer = TIM8,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_PuPd = GPIO_PuPd_DOWN
},
.pin_source = GPIO_PinSource8,
},
.remap = GPIO_AF_TIM8,
},
};
const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_hcsr04_port_all_channels,
.num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
.trigger = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
};
#endif
#if defined(PIOS_INCLUDE_USB)
#include "pios_usb_priv.h"

2
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -94,6 +94,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )

21
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -293,12 +293,17 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_AUX_RX_BUF_LEN 512
#define PIOS_COM_AUX_TX_BUF_LEN 512
#define PIOS_COM_HKOSD_RX_BUF_LEN 22
#define PIOS_COM_HKOSD_TX_BUF_LEN 22
uint32_t pios_com_aux_id = 0;
uint32_t pios_com_gps_id = 0;
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telem_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
/*
* Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only
@ -638,6 +643,10 @@ void PIOS_Board_Init(void) {
case HWSETTINGS_RV_AUXPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
case HWSETTINGS_RV_AUXPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rv_auxport */
/* Configure AUXSbusPort */
//TODO: ensure that the serial invertion pin is setted correctly
@ -700,6 +709,9 @@ void PIOS_Board_Init(void) {
case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
case HWSETTINGS_RV_AUXSBUSPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rv_auxport */
/* Configure FlexiPort */
@ -818,6 +830,15 @@ void PIOS_Board_Init(void) {
#endif
#if defined(PIOS_INCLUDE_HCSR04)
{
PIOS_TIM_InitClock(&tim_8_cfg);
uint32_t pios_hcsr04_id;
PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg);
}
#endif
#if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize();
uint32_t pios_gcsrcvr_id;

38
flight/targets/boards/revoproto/pios_board.h
View File

@ -35,18 +35,18 @@
// Timers and Channels Used
//------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
//------------------------
// DMA Channels Used
@ -71,7 +71,6 @@ TIM8 | | | |
#define BOARD_WRITABLE true
#define MAX_DEL_RETRYS 3
//------------------------
// PIOS_LED
//------------------------
@ -94,6 +93,7 @@ TIM8 | | | |
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_SENSORS 0x0010
#define PIOS_WDG_AUTOTUNE 0x0020
//------------------------
// PIOS_I2C
@ -125,6 +125,7 @@ extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
extern uint32_t pios_com_vcp_id;
extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
@ -132,6 +133,7 @@ extern uint32_t pios_com_vcp_id;
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX
#define PIOS_COM_OSDHK (pios_com_hkosd_id)
//------------------------
// TELEMETRY
@ -173,7 +175,6 @@ extern uint32_t pios_com_vcp_id;
//
#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK
//-------------------------
// Interrupt Priorities
//-------------------------
@ -181,7 +182,6 @@ extern uint32_t pios_com_vcp_id;
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------
// PIOS_RCVR
// See also pios_board.c
@ -235,8 +235,9 @@ extern uint32_t pios_com_vcp_id;
// ADC
// PIOS_ADC_PinGet(0) = Current sensor
// PIOS_ADC_PinGet(1) = Voltage sensor
// PIOS_ADC_PinGet(4) = VREF
// PIOS_ADC_PinGet(5) = Temperature sensor
// PIOS_ADC_PinGet(2) = VREF
// PIOS_ADC_PinGet(3) = Temperature sensor
// PIOS_ADC_PinGet(4) = Board Power
//-------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
@ -253,7 +254,8 @@ extern uint32_t pios_com_vcp_id;
#define PIOS_ADC_NUM_CHANNELS 4
#define PIOS_ADC_MAX_OVERSAMPLING 2
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f
#define PIOS_ADC_USE_TEMP_SENSOR 1
//-------------------------
// USB

2
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -83,6 +83,8 @@ UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity

30
flight/uavobjects/uavobjectmanager.c
View File

@ -182,7 +182,11 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb);
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD) && defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
#error Both PIOS_USE_SETTINGS_ON_SDCARD and PIOS_INCLUDE_FLASH_SECTOR_SETTINGS. Only one settings storage allowed.
#endif
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename);
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
#endif
@ -697,7 +701,7 @@ unlock_exit:
return rc;
}
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
/**
* Save the data of the specified object instance to the file system (SD card).
* The object will be appended and the file will not be closed.
@ -772,7 +776,7 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId,
xSemaphoreGiveRecursive(mutex);
return 0;
}
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
/**
* Save the data of the specified object to the file system (SD card).
@ -808,7 +812,7 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
return -1;
}
#endif
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
FILEINFO file;
uint8_t filename[14];
@ -836,11 +840,11 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
return 0;
}
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
/**
* Load an object from the file system (SD card).
* @param[in] file File to read from
@ -928,7 +932,7 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
xSemaphoreGiveRecursive(mutex);
return obj_handle;
}
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
/**
* Load an object from the file system (SD card).
@ -968,7 +972,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
#endif
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
FILEINFO file;
UAVObjHandle loadedObj;
uint8_t filename[14];
@ -1004,7 +1008,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
return 0;
}
@ -1020,7 +1024,7 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
PIOS_FLASHFS_ObjDelete(0, UAVObjGetID(obj_handle), instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
uint8_t filename[14];
// Check for file system availability
@ -1038,7 +1042,7 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
return 0;
}
@ -2012,7 +2016,7 @@ static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
return -1;
}
#if defined(PIOS_INCLUDE_SDCARD)
#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
/**
* Wrapper for the sprintf function
*/
@ -2030,4 +2034,4 @@ static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename)
{
customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
}
#endif /* PIOS_INCLUDE_SDCARD */
#endif /* PIOS_USE_SETTINGS_ON_SDCARD */

14
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp
View File

@ -28,7 +28,7 @@
#include "gpsitem.h"
namespace mapcontrol
{
GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true)
GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true)
,autosetdistance(100)
{
pic.load(uavPic);
@ -42,8 +42,10 @@ namespace mapcontrol
trailLine=new QGraphicsItemGroup(this);
trailLine->setParentItem(map);
this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
setCacheMode(QGraphicsItem::ItemCoordinateCache);
mapfollowtype=UAVMapFollowType::None;
trailtype=UAVTrailType::ByDistance;
//trailtype=UAVTrailType::ByDistance;
trailtype=UAVTrailType::ByTimeElapsed;
timer.start();
connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos()));
connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal)));
@ -74,12 +76,12 @@ namespace mapcontrol
{
if(timer.elapsed()>trailtime*1000)
{
TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
trail->addToGroup(ob);
connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
if(!lasttrailline.IsEmpty())
{
TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
trailLine->addToGroup(obj);
connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
}
@ -92,12 +94,12 @@ namespace mapcontrol
{
if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance)
{
TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
trail->addToGroup(ob);
connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
if(!lasttrailline.IsEmpty())
{
TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
trailLine->addToGroup(obj);
connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
}

2
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
View File

@ -80,6 +80,7 @@ namespace mapcontrol
if(GPS!=0)
{
GPS->DeleteTrail();
delete GPS;
GPS=0;
}
@ -93,6 +94,7 @@ namespace mapcontrol
{
GPS=new GPSItem(map,this);
GPS->setParentItem(map);
GPS->setOpacity(overlayOpacity);
setOverlayOpacity(overlayOpacity);
}
}

2
ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini
View File

@ -25,7 +25,7 @@ cc_channel_6 = Ch13-FPV-Tilt
;cc_channel_10=
; Control TX or RX (before or after mixes)
send_to = RX
send_to = TX
[Output]

2
ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp
View File

@ -87,7 +87,7 @@ void UdpSender::sendDatagram(const simToPlugin *stp)
// attitude
out << stp->heading << stp->pitch << stp->roll;
// electric
out << stp->voltage << stp->current;
out << stp->voltage << stp->current << stp->consumedCharge;
// matrix
out << stp->axisXx << stp->axisXy << stp->axisXz;
out << stp->axisYx << stp->axisYy << stp->axisYz;

80
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
View File

@ -49,10 +49,11 @@ bool AeroSimRCSimulator::setupProcess()
void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
{
Q_UNUSED(outPort)
if (inSocket->bind(QHostAddress(host), inPort))
if (inSocket->bind(QHostAddress(host), inPort)) {
emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
else
} else {
emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
}
}
void AeroSimRCSimulator::transmitUpdate()
@ -71,13 +72,45 @@ void AeroSimRCSimulator::transmitUpdate()
channels[i] = out;
}
ActuatorDesired::DataFields actData;
FlightStatus::DataFields flightStatusData = flightStatus->getData();
ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
float roll = -1;
float pitch = -1;
float yaw = -1;
float throttle = -1;
if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
// Read joystick input
if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
// Note: Pitch sign is reversed in FG ?
roll = manCtrlData.Roll;
pitch = -manCtrlData.Pitch;
yaw = manCtrlData.Yaw;
throttle = manCtrlData.Throttle;
}
} else {
// Read ActuatorDesired from autopilot
actData = actDesired->getData();
roll = actData.Roll;
pitch = -actData.Pitch;
yaw = actData.Yaw;
throttle = (actData.Throttle*2.0)-1.0;
}
channels[0] = roll;
channels[1] = pitch;
if(throttle < -1) {
throttle = -1;
}
channels[2] = throttle;
channels[3] = yaw;
// read flight status
FlightStatus::DataFields flightData;
flightData = flightStatus->getData();
quint8 armed;
quint8 mode;
armed = flightData.Armed;
mode = flightData.FlightMode;
armed = flightStatusData.Armed;
mode = flightStatusData.FlightMode;
QByteArray data;
// 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32)
@ -85,8 +118,9 @@ void AeroSimRCSimulator::transmitUpdate()
QDataStream stream(&data, QIODevice::WriteOnly);
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
stream << quint32(0x52434D44); // magic header, "RCMD"
for (int i = 0; i < 10; ++i)
for (int i = 0; i < 10; ++i) {
stream << channels[i]; // channels
}
stream << armed << mode; // flight status
stream << udpCounterASrecv; // packet counter
@ -108,7 +142,7 @@ void AeroSimRCSimulator::transmitUpdate()
void AeroSimRCSimulator::processUpdate(const QByteArray &data)
{
// check size
if (data.size() > 188) {
if (data.size() > 192) {
qDebug() << "!!! big datagram: " << data.size();
return;
}
@ -132,10 +166,11 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
posX, posY, posZ, // world
velX, velY, velZ, // world
angX, angY, angZ, // model
accX, accY, accZ, // model
lat, lon, agl, // world
accX, accY, accZ; // model
qreal lat, lon;
float agl, // world
yaw, pitch, roll, // model
volt, curr,
volt, curr, cons,
rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
ch[AEROSIM_RCCHANNEL_NUMELEM];
@ -148,7 +183,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
stream >> accX >> accY >> accZ;
stream >> lat >> lon >> agl;
stream >> yaw >> pitch >> roll;
stream >> volt >> curr;
stream >> volt >> curr >> cons;
stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
stream >> udpCounterASrecv;
@ -157,10 +192,10 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
memset(&out, 0, sizeof(Output2Hardware));
out.delT=delT;
out.delT = delT;
/*************************************************************************************/
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) {
out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1
}
@ -209,17 +244,20 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
out.groundspeed = qSqrt(velX * velX + velY * velY);
/**********************************************************************************************/
out.dstN = posY * 100;
out.dstE = posX * 100;
out.dstD = posZ * -100;
out.dstN = posY;
out.dstE = posX;
out.dstD = -posZ;
out.velDown = velY * 100;
out.velEast = velX * 100;
out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`???
out.velNorth = velY;
out.velEast = velX;
out.velDown = -velZ;
out.voltage = volt;
out.current = curr;
out.consumption = cons * 1000.0;
updateUAVOs(out);
#ifdef DBG_TIMERS
static int cntRX = 0;
if (cntRX >= 100) {

2
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
View File

@ -809,7 +809,7 @@
<item>
<widget class="QGroupBox" name="baroAltitudeCheckbox">
<property name="enabled">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="title">
<string>BaroAltitude</string>

20
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

@ -69,6 +69,7 @@ Simulator::Simulator(const SimulatorSettings& params) :
gcsRcvrTime = currentTime;
attRawTime = currentTime;
baroAltTime = currentTime;
battTime = currentTime;
airspeedActualTime=currentTime;
//Define standard atmospheric constants
@ -153,6 +154,7 @@ void Simulator::onStart()
velActual = VelocityActual::GetInstance(objManager);
posActual = PositionActual::GetInstance(objManager);
baroAlt = BaroAltitude::GetInstance(objManager);
flightBatt = FlightBatteryState::GetInstance(objManager);
airspeedActual = AirspeedActual::GetInstance(objManager);
attActual = AttitudeActual::GetInstance(objManager);
attSettings = AttitudeSettings::GetInstance(objManager);
@ -284,7 +286,10 @@ void Simulator::setupObjects()
setupOutputObject(airspeedActual, settings.airspeedActualRate);
if(settings.baroAltitudeEnabled)
{
setupOutputObject(baroAlt, settings.baroAltRate);
setupOutputObject(flightBatt, settings.baroAltRate);
}
}
@ -726,6 +731,21 @@ void Simulator::updateUAVOs(Output2Hardware out){
}
}
/*******************************/
// Update FlightBatteryState object
if (settings.baroAltitudeEnabled){
if (battTime.msecsTo(currentTime) >= settings.baroAltRate) {
FlightBatteryState::DataFields batteryData;
memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields));
batteryData.Voltage = out.voltage;
batteryData.Current = out.current;
batteryData.ConsumedEnergy = out.consumption;
flightBatt->setData(batteryData);
battTime=battTime.addMSecs(settings.baroAltRate);
}
}
/*******************************/
// Update AirspeedActual object
if (settings.airspeedActualEnabled){

7
ground/openpilotgcs/src/plugins/hitl/simulator.h
View File

@ -45,6 +45,7 @@
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "flightbatterystate.h"
#include "flightstatus.h"
#include "gcsreceiver.h"
#include "gcstelemetrystats.h"
@ -194,6 +195,10 @@ struct Output2Hardware{
float pitchDesired;
float yawDesired;
float throttleDesired;
float voltage;
float current;
float consumption;
};
//struct Output2Simulator{
@ -285,6 +290,7 @@ protected:
ActuatorDesired* actDesired;
ManualControlCommand* manCtrlCommand;
FlightStatus* flightStatus;
FlightBatteryState* flightBatt;
BaroAltitude* baroAlt;
AirspeedActual* airspeedActual;
AttitudeActual* attActual;
@ -323,6 +329,7 @@ private:
QTime gpsPosTime;
QTime groundTruthTime;
QTime baroAltTime;
QTime battTime;
QTime gcsRcvrTime;
QTime airspeedActualTime;

23
ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
View File

@ -200,6 +200,17 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
ui->dsb_condParam4->setVisible(false);
ui->condParam1->setText("Relative Distance(0=complete,1=just starting)");
break;
case MapDataDelegate::ENDCONDITION_BELOWERROR:
ui->condParam1->setVisible(true);
ui->condParam2->setVisible(false);
ui->condParam3->setVisible(false);
ui->condParam4->setVisible(false);
ui->dsb_condParam1->setVisible(true);
ui->dsb_condParam2->setVisible(false);
ui->dsb_condParam3->setVisible(false);
ui->dsb_condParam4->setVisible(false);
ui->condParam1->setText("error margin (in m)");
break;
case MapDataDelegate::ENDCONDITION_ABOVEALTITUDE:
ui->condParam1->setVisible(true);
ui->condParam2->setVisible(false);
@ -211,6 +222,18 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
ui->dsb_condParam4->setVisible(false);
ui->condParam1->setText("Altitude in meters (negative)");
break;
case MapDataDelegate::ENDCONDITION_ABOVESPEED:
ui->condParam1->setVisible(true);
ui->condParam2->setVisible(true);
ui->condParam3->setVisible(false);
ui->condParam4->setVisible(false);
ui->dsb_condParam1->setVisible(true);
ui->dsb_condParam2->setVisible(true);
ui->dsb_condParam3->setVisible(false);
ui->dsb_condParam4->setVisible(false);
ui->condParam1->setText("Speed in meters/second");
ui->condParam2->setText("flag: 0=groundspeed 1=airspeed");
break;
case MapDataDelegate::ENDCONDITION_POINTINGTOWARDSNEXT:
ui->condParam1->setVisible(true);
ui->condParam2->setVisible(false);

8
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -24,6 +24,7 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#define USE_PATHPLANNER
#include "opmapgadgetwidget.h"
#include "ui_opmap_widget.h"
@ -546,8 +547,8 @@ void OPMapGadgetWidget::closeEvent(QCloseEvent *event)
*/
void OPMapGadgetWidget::updatePosition()
{
double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
double gps_latitude, gps_longitude, gps_altitude, gps_heading;
double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
double gps_latitude, gps_longitude, gps_altitude, gps_heading;
internals::PointLatLng uav_pos;
internals::PointLatLng gps_pos;
@ -581,7 +582,7 @@ void OPMapGadgetWidget::updatePosition()
gps_longitude = gpsPositionData.Longitude;
gps_altitude = gpsPositionData.Altitude;
gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7);
//**********************
// get the current position and heading estimates
@ -643,6 +644,7 @@ void OPMapGadgetWidget::updatePosition()
{
m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position
m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading
m_map->GPS->update();
}
m_map->UAV->updateTextOverlay();
m_map->UAV->update();

2
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
View File

@ -121,7 +121,9 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
combo->addItem("Timeout",ENDCONDITION_TIMEOUT);
combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET);
combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING);
combo->addItem("Below Error",ENDCONDITION_BELOWERROR);
combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE);
combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED);
combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT);
combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT);
combo->addItem("Immediate",ENDCONDITION_IMMEDIATE);

5
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
View File

@ -41,8 +41,9 @@ class MapDataDelegate : public QItemDelegate
MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7,
MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions;
typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2,
ENDCONDITION_LEGREMAINING=3, ENDCONDITION_ABOVEALTITUDE=4, ENDCONDITION_POINTINGTOWARDSNEXT=5,
ENDCONDITION_PYTHONSCRIPT=6, ENDCONDITION_IMMEDIATE=7 } EndConditionOptions;
ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5,
ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8,
ENDCONDITION_IMMEDIATE=9 } EndConditionOptions;
typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1,
COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3,
COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions;

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -95,6 +95,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/txpidsettings.h \
$$UAVOBJECT_SYNTHETICS/cameradesired.h \
$$UAVOBJECT_SYNTHETICS/faultsettings.h \
$$UAVOBJECT_SYNTHETICS/poilearnsettings.h \
$$UAVOBJECT_SYNTHETICS/poilocation.h \
$$UAVOBJECT_SYNTHETICS/oplinksettings.h \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
$$UAVOBJECT_SYNTHETICS/osdsettings.h \
@ -175,6 +177,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
$$UAVOBJECT_SYNTHETICS/cameradesired.cpp \
$$UAVOBJECT_SYNTHETICS/faultsettings.cpp \
$$UAVOBJECT_SYNTHETICS/poilearnsettings.cpp \
$$UAVOBJECT_SYNTHETICS/poilocation.cpp \
$$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
$$UAVOBJECT_SYNTHETICS/osdsettings.cpp \

3
make/boot-defs.mk
View File

@ -112,9 +112,6 @@ BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
# Compiler flags
CDEFS += $(BLONLY_CDEFS)

5
make/common-defs.mk
View File

@ -95,6 +95,11 @@ endif
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Provide board-specific defines
CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

14
shared/uavobjectdefinition/hwsettings.xml
View File

@ -2,19 +2,19 @@
<object name="HwSettings" singleinstance="true" settings="true">
<description>Selection of optional hardware configurations.</description>
<field name="CC_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Telemetry"/>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,PPM,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Disabled"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Telemetry"/>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,PPM,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RV_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
<field name="RV_AuxPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
<field name="RV_AuxSBusPort" units="function" type="enum" elements="1" options="Disabled,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
<field name="RV_AuxPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RV_AuxSBusPort" units="function" type="enum" elements="1" options="Disabled,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RV_FlexiPort" units="function" type="enum" elements="1" options="Disabled,I2C,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
<field name="RV_TelemetryPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,ComAux,ComBridge" defaultvalue="Telemetry"/>
<field name="RV_GPSPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,ComAux,ComBridge" defaultvalue="GPS"/>
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Telemetry"/>
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Disabled"/>
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Telemetry"/>
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RadioPort" units="function" type="enum" elements="1" options="Disabled,Telemetry" defaultvalue="Disabled"/>
<field name="MaxRFPower" units="mW" type="enum" elements="1" options="1.25,1.6,3.16,6.3,12.6,25,50,100" defaultvalue="100"/>
@ -26,7 +26,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="ADCRouting" units="" type="enum" elementnames="ADC0,ADC1,ADC2,ADC3" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>

38
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -43,32 +43,32 @@
units=""
type="enum"
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold"
limits="\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner"/>
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="FailsafeBehavior" units="" type="enum" elements="1" options="None" defaultvalue="None"/>

3
shared/uavobjectdefinition/osdsettings.xml
View File

@ -14,6 +14,9 @@
<field name="Heading" units="" type="enum" elements="1" options="Disabled,Enabled" defaultvalue="Enabled"/>
<field name="HeadingSetup" units="" type="int16" elements="2" elementnames="X,Y" defaultvalue="168,240"/>
<field name="Screen" units="" type="uint8" elements="1" defaultvalue="0"/>
<field name="White" units="" type="uint8" elements="1" defaultvalue="4"/>
<field name="Black" units="" type="uint8" elements="1" defaultvalue="1"/>
<field name="AltitudeSource" units="" type="enum" elements="1" options="GPS,Baro" defaultvalue="GPS"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

10
shared/uavobjectdefinition/poilearnsettings.xml Normal file
View File

@ -0,0 +1,10 @@
<xml>
<object name="PoiLearnSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Point of Interest feature</description>
<field name="Input" units="channel" type="enum" elements="1" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

12
shared/uavobjectdefinition/poilocation.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="PoiLocation" singleinstance="true" settings="false">
<description>Contains the current Point of interest relative to @ref HomeLocation</description>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -5,7 +5,7 @@
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

6
shared/uavobjectdefinition/taskinfo.xml
View File

@ -27,6 +27,8 @@
<elementname>ModemStat</elementname>
<elementname>Autotune</elementname>
<elementname>EventDispatcher</elementname>
<elementname>MagBaro</elementname>
<elementname>OSDGen</elementname>
<elementname>CallbackScheduler0</elementname>
<elementname>CallbackScheduler1</elementname>
<elementname>CallbackScheduler2</elementname>
@ -59,6 +61,8 @@
<elementname>ModemStat</elementname>
<elementname>Autotune</elementname>
<elementname>EventDispatcher</elementname>
<elementname>MagBaro</elementname>
<elementname>OSDGen</elementname>
<elementname>CallbackScheduler0</elementname>
<elementname>CallbackScheduler1</elementname>
<elementname>CallbackScheduler2</elementname>
@ -95,6 +99,8 @@
<elementname>ModemStat</elementname>
<elementname>Autotune</elementname>
<elementname>EventDispatcher</elementname>
<elementname>MagBaro</elementname>
<elementname>OSDGen</elementname>
<elementname>CallbackScheduler0</elementname>
<elementname>CallbackScheduler1</elementname>
<elementname>CallbackScheduler2</elementname>