diff --git a/androidgcs/res/layout/mycustommapview.xml b/androidgcs/res/layout/mycustommapview.xml
new file mode 100644
index 000000000..0bfdf9842
--- /dev/null
+++ b/androidgcs/res/layout/mycustommapview.xml
@@ -0,0 +1,38 @@
+
+
+
+
+
diff --git a/androidgcs/res/menu/map_menu.xml b/androidgcs/res/menu/map_menu.xml
new file mode 100644
index 000000000..77de36c5b
--- /dev/null
+++ b/androidgcs/res/menu/map_menu.xml
@@ -0,0 +1,9 @@
+
+
+
diff --git a/androidgcs/res/values/strings.xml b/androidgcs/res/values/strings.xml
index 37e54515a..fc6237377 100644
--- a/androidgcs/res/values/strings.xml
+++ b/androidgcs/res/values/strings.xml
@@ -32,6 +32,9 @@
Alarms
TxRate:
RxRate:
+ Latitude:
+ Longitude:
+ Altitude:
Tester
3DView
Tuning
diff --git a/androidgcs/src/org/openpilot/androidgcs/Controller.java b/androidgcs/src/org/openpilot/androidgcs/Controller.java
index 32b9e2655..a4bad8ee7 100644
--- a/androidgcs/src/org/openpilot/androidgcs/Controller.java
+++ b/androidgcs/src/org/openpilot/androidgcs/Controller.java
@@ -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() {
diff --git a/androidgcs/src/org/openpilot/androidgcs/MyCustomMapView.java b/androidgcs/src/org/openpilot/androidgcs/MyCustomMapView.java
new file mode 100644
index 000000000..8ba53d168
--- /dev/null
+++ b/androidgcs/src/org/openpilot/androidgcs/MyCustomMapView.java
@@ -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();
+ }
+}
+}
diff --git a/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java b/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java
index 6ee1bdbcd..cddad46f7 100644
--- a/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java
+++ b/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java
@@ -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 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);
diff --git a/flight/Project/gdb/osd b/flight/Project/gdb/osd
new file mode 100644
index 000000000..291b0570a
--- /dev/null
+++ b/flight/Project/gdb/osd
@@ -0,0 +1,4 @@
+define connect
+ target remote localhost:3333
+ file ./build/fw_osd/fw_osd.elf
+end
diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c
index 72b0e1d83..21151548e 100644
--- a/flight/libraries/paths.c
+++ b/flight/libraries/paths.c
@@ -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);
}
diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c
index 75c1fd6b2..3207604e6 100644
--- a/flight/libraries/sanitycheck.c
+++ b/flight/libraries/sanitycheck.c
@@ -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;
}
diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c
index 4e11848e7..04c2ac48c 100644
--- a/flight/modules/Altitude/altitude.c
+++ b/flight/modules/Altitude/altitude.c
@@ -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);
diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c
index 54b241408..1cee36418 100644
--- a/flight/modules/Altitude/revolution/altitude.c
+++ b/flight/modules/Altitude/revolution/altitude.c
@@ -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;
diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c
index 6f5bb2f08..922ec269e 100644
--- a/flight/modules/Attitude/revolution/attitude.c
+++ b/flight/modules/Attitude/revolution/attitude.c
@@ -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);
diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c
index ab02a902f..99df558fe 100644
--- a/flight/modules/Extensions/MagBaro/magbaro.c
+++ b/flight/modules/Extensions/MagBaro/magbaro.c
@@ -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);
diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h
index 4977fad2d..7b841a035 100644
--- a/flight/modules/ManualControl/inc/manualcontrol.h
+++ b/flight/modules/ManualControl/inc/manualcontrol.h
@@ -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 ( \
diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index de3ca578a..479ea221c 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -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(¤tDown);
- 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(¤tDown);
+ 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();
}
-
-
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/modules/Osd/WavPlayer/inc/wavplayer.h b/flight/modules/Osd/WavPlayer/inc/wavplayer.h
index c2d1f7c65..719e32c3a 100644
--- a/flight/modules/Osd/WavPlayer/inc/wavplayer.h
+++ b/flight/modules/Osd/WavPlayer/inc/wavplayer.h
@@ -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_ */
diff --git a/flight/modules/Osd/WavPlayer/wavplayer.c b/flight/modules/Osd/WavPlayer/wavplayer.c
index f0fd97a7a..1788f0a97 100644
--- a/flight/modules/Osd/WavPlayer/wavplayer.c
+++ b/flight/modules/Osd/WavPlayer/wavplayer.c
@@ -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;
+ }
}
-
// ****************
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/modules/Osd/osdgen/inc/osdgen.h b/flight/modules/Osd/osdgen/inc/osdgen.h
index 98193ed18..92f8d6cd3 100644
--- a/flight/modules/Osd/osdgen/inc/osdgen.h
+++ b/flight/modules/Osd/osdgen/inc/osdgen.h
@@ -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_ */
diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c
index cbe067f0f..9172cc006 100644
--- a/flight/modules/Osd/osdgen/osdgen.c
+++ b/flight/modules/Osd/osdgen/osdgen.c
@@ -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= -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;isize)
- 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;isize)
- 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;isize)
- 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;isize)
- 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) {
+ 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);
+ }
}
-
// ****************
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/modules/Osd/osdinput/inc/osdinput.h b/flight/modules/Osd/osdinput/inc/osdinput.h
index 58e506ae1..e98386beb 100644
--- a/flight/modules/Osd/osdinput/inc/osdinput.h
+++ b/flight/modules/Osd/osdinput/inc/osdinput.h
@@ -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_ */
diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c
index b0a6acd89..abde6bd22 100644
--- a/flight/modules/Osd/osdinput/osdinput.c
+++ b/flight/modules/Osd/osdinput/osdinput.c
@@ -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);
+ }
}
-
// ****************
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/modules/Osd/osdoutput/inc/osdoutput.h b/flight/modules/Osd/osdoutput/inc/osdoutput.h
new file mode 100644
index 000000000..a6f2e80bd
--- /dev/null
+++ b/flight/modules/Osd/osdoutput/inc/osdoutput.h
@@ -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 */
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c
new file mode 100644
index 000000000..5285a88a8
--- /dev/null
+++ b/flight/modules/Osd/osdoutput/osdoutput.c
@@ -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)
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c
index 223925c74..32f99552a 100644
--- a/flight/modules/PathPlanner/pathplanner.c
+++ b/flight/modules/PathPlanner/pathplanner.c
@@ -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);
diff --git a/flight/modules/Stabilization/inc/relay_tuning.h b/flight/modules/Stabilization/inc/relay_tuning.h
index 9044986c5..2fd69cafa 100644
--- a/flight/modules/Stabilization/inc/relay_tuning.h
+++ b/flight/modules/Stabilization/inc/relay_tuning.h
@@ -36,4 +36,4 @@
int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
-#endif
\ No newline at end of file
+#endif
diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c
index 5d53aa7d3..02081f427 100644
--- a/flight/modules/Stabilization/relay_tuning.c
+++ b/flight/modules/Stabilization/relay_tuning.c
@@ -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;
}
-
diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c
index f76eeff1f..d9a1c3891 100644
--- a/flight/modules/Stabilization/stabilization.c
+++ b/flight/modules/Stabilization/stabilization.c
@@ -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);
diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c
index cc71809de..6ef2e9445 100644
--- a/flight/modules/System/systemmod.c
+++ b/flight/modules/System/systemmod.c
@@ -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
}
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c
index 2d7b3ae1f..a80d7e750 100644
--- a/flight/modules/VtolPathFollower/vtolpathfollower.c
+++ b/flight/modules/VtolPathFollower/vtolpathfollower.c
@@ -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);
+ }
+ }
+ }
}
diff --git a/flight/pios/common/pios_hcsr04.c b/flight/pios/common/pios_hcsr04.c
index 02177fac0..925c5a175 100644
--- a/flight/pios/common/pios_hcsr04.c
+++ b/flight/pios/common/pios_hcsr04.c
@@ -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 */
diff --git a/flight/pios/common/pios_video.c b/flight/pios/common/pios_video.c
index cabdef749..8e211d68c 100644
--- a/flight/pios/common/pios_video.c
+++ b/flight/pios/common/pios_video.c
@@ -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_numpixel_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 */
diff --git a/flight/pios/inc/pios_hcsr04.h b/flight/pios/inc/pios_hcsr04.h
index d2574a826..fe735e336 100644
--- a/flight/pios/inc/pios_hcsr04.h
+++ b/flight/pios/inc/pios_hcsr04.h
@@ -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);
diff --git a/flight/pios/inc/pios_hcsr04_priv.h b/flight/pios/inc/pios_hcsr04_priv.h
new file mode 100644
index 000000000..c53bdddf1
--- /dev/null
+++ b/flight/pios/inc/pios_hcsr04_priv.h
@@ -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
+#include
+
+#include
+
+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 */
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/pios/inc/pios_video.h b/flight/pios/inc/pios_video.h
index c5c4e0d23..1689b0a09 100644
--- a/flight/pios/inc/pios_video.h
+++ b/flight/pios/inc/pios_video.h
@@ -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
diff --git a/flight/pios/stm32f10x/pios_tim.c b/flight/pios/stm32f10x/pios_tim.c
index d160c2c51..28a187341 100644
--- a/flight/pios/stm32f10x/pios_tim.c
+++ b/flight/pios/stm32f10x/pios_tim.c
@@ -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);
diff --git a/flight/pios/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c
index 65c30e1aa..111067471 100644
--- a/flight/pios/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c
+++ b/flight/pios/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c
@@ -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.
- *
- * © COPYRIGHT 2011 STMicroelectronics © COPYRIGHT 2011 STMicroelectronics
-* Disable all RTOS tasks
-* Disable all interrupts
-* Turn off all board LEDs
-* Reset STM32
-*
-* \return < 0 if reset failed
-*/
+ * Shutdown PIOS and reset the microcontroller:
+ * Disable all RTOS tasks
+ * Disable all interrupts
+ * Turn off all board LEDs
+ * Reset STM32
+ *
+ * \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 */
/**
- * @}
- * @}
- */
+ * @}
+ * @}
+ */
diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c
index f8819c725..b4f83f541 100644
--- a/flight/targets/boards/coptercontrol/board_hw_defs.c
+++ b/flight/targets/boards/coptercontrol/board_hw_defs.c
@@ -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
+
+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
diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile
index d697f7894..a4b11889a 100644
--- a/flight/targets/boards/coptercontrol/firmware/Makefile
+++ b/flight/targets/boards/coptercontrol/firmware/Makefile
@@ -43,6 +43,7 @@ OPTMODULES += CameraStab
OPTMODULES += ComUsbBridge
OPTMODULES += GPS
OPTMODULES += TxPID
+OPTMODULES += Osd/osdoutput
#OPTMODULES += Altitude
#OPTMODULES += Fault
diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c
index d84f8566b..da067b39c 100644
--- a/flight/targets/boards/coptercontrol/firmware/pios_board.c
+++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c
@@ -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)
diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h
index 0567e6622..92ad35f7b 100644
--- a/flight/targets/boards/coptercontrol/pios_board.h
+++ b/flight/targets/boards/coptercontrol/pios_board.h
@@ -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
//-------------------------
diff --git a/flight/targets/boards/osd/board-info.mk b/flight/targets/boards/osd/board-info.mk
index 3e707366c..c81fd28cc 100644
--- a/flight/targets/boards/osd/board-info.mk
+++ b/flight/targets/boards/osd/board-info.mk
@@ -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
\ No newline at end of file
+SYSCLK_FREQ := 168000000
diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c
index 4731a490d..390e672e7 100644
--- a/flight/targets/boards/osd/board_hw_defs.c
+++ b/flight/targets/boards/osd/board_hw_defs.c
@@ -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
@@ -200,6 +224,7 @@ void PIOS_SPI_sdcard_irq_handler(void)
#include
#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
+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
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
diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile
index 3bfa9bf47..fb2b56603 100644
--- a/flight/targets/boards/osd/firmware/Makefile
+++ b/flight/targets/boards/osd/firmware/Makefile
@@ -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
diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h
index 3772c5939..628f6d326 100644
--- a/flight/targets/boards/osd/firmware/inc/pios_config.h
+++ b/flight/targets/boards/osd/firmware/inc/pios_config.h
@@ -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 */
diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c
index edc9aba1f..1edd83dd8 100644
--- a/flight/targets/boards/osd/firmware/pios_board.c
+++ b/flight/targets/boards/osd/firmware/pios_board.c
@@ -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
}
diff --git a/flight/targets/boards/osd/pios_board.h b/flight/targets/boards/osd/pios_board.h
index 88ab5c0d9..c4d1e34b5 100644
--- a/flight/targets/boards/osd/pios_board.h
+++ b/flight/targets/boards/osd/pios_board.h
@@ -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
diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c
index 5b54627a3..4ac4d8062 100644
--- a/flight/targets/boards/revolution/board_hw_defs.c
+++ b/flight/targets/boards/revolution/board_hw_defs.c
@@ -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
diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile
index ce8905585..5309b2ab8 100644
--- a/flight/targets/boards/revolution/firmware/Makefile
+++ b/flight/targets/boards/revolution/firmware/Makefile
@@ -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 =
diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc
index 56f708ebe..f750288c0 100644
--- a/flight/targets/boards/revolution/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc
@@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c
index ffdb8b9c5..41baaf0d9 100644
--- a/flight/targets/boards/revolution/firmware/pios_board.c
+++ b/flight/targets/boards/revolution/firmware/pios_board.c
@@ -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)
diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h
index f58ace784..3d6688ddb 100644
--- a/flight/targets/boards/revolution/pios_board.h
+++ b/flight/targets/boards/revolution/pios_board.h
@@ -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
diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c
index ed39f2864..493df060a 100644
--- a/flight/targets/boards/revoproto/board_hw_defs.c
+++ b/flight/targets/boards/revoproto/board_hw_defs.c
@@ -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
@@ -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
+
+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"
diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
index 1d7b592f8..f025cd9ea 100644
--- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
@@ -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 )
diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c
index d133c878e..c11651a7c 100644
--- a/flight/targets/boards/revoproto/firmware/pios_board.c
+++ b/flight/targets/boards/revoproto/firmware/pios_board.c
@@ -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;
diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h
index e10c610ce..0467cd1fc 100644
--- a/flight/targets/boards/revoproto/pios_board.h
+++ b/flight/targets/boards/revoproto/pios_board.h
@@ -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
diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc
index 4a6c453ac..23ffde045 100644
--- a/flight/targets/boards/simposix/firmware/UAVObjects.inc
+++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc
@@ -83,6 +83,8 @@ UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c
index 8761d875a..16f724d87 100644
--- a/flight/uavobjects/uavobjectmanager.c
+++ b/flight/uavobjects/uavobjectmanager.c
@@ -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 */
diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp
index 07d75da2a..358ba5ed7 100644
--- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp
+++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp
@@ -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()));
}
diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
index bdc60225d..ea477adde 100644
--- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
+++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
@@ -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);
}
}
diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini
index 8b4174f60..555ee0aa7 100644
--- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini
+++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini
@@ -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]
diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp
index 0448a705a..44151b661 100644
--- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp
+++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp
@@ -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;
diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
index aedf3e909..09f96baa5 100644
--- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
+++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
@@ -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) {
diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
index 4b8d6e6e5..543feff6a 100644
--- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
+++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
@@ -809,7 +809,7 @@
-
- false
+ true
BaroAltitude
diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp
index fc1e81587..01dbfa136 100644
--- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp
+++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp
@@ -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){
diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h
index b819520aa..c4795142c 100644
--- a/ground/openpilotgcs/src/plugins/hitl/simulator.h
+++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h
@@ -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;
diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
index 459ff1c32..73a1e58e8 100644
--- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
@@ -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);
diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
index 0d9639e38..223343ec4 100644
--- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
@@ -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();
diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
index 7bd385842..fd44550a6 100644
--- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
@@ -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);
diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
index ae29bc975..53aac4887 100644
--- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
+++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
@@ -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;
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 5b61364fc..42536c390 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -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 \
diff --git a/make/boot-defs.mk b/make/boot-defs.mk
index 3ddb445c7..eca39d078 100644
--- a/make/boot-defs.mk
+++ b/make/boot-defs.mk
@@ -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)
diff --git a/make/common-defs.mk b/make/common-defs.mk
index d31440bdf..67fd3074a 100644
--- a/make/common-defs.mk
+++ b/make/common-defs.mk
@@ -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
diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml
index 5687feee6..765e15d66 100644
--- a/shared/uavobjectdefinition/flightstatus.xml
+++ b/shared/uavobjectdefinition/flightstatus.xml
@@ -4,7 +4,7 @@
Selection of optional hardware configurations.
+
+ Settings for the @ref Point of Interest feature
+
+
diff --git a/shared/uavobjectdefinition/poilocation.xml b/shared/uavobjectdefinition/poilocation.xml
new file mode 100644
index 000000000..70022a86a
--- /dev/null
+++ b/shared/uavobjectdefinition/poilocation.xml
@@ -0,0 +1,12 @@
+
+
+ Contains the current Point of interest relative to @ref HomeLocation
+
+
diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml
index 74dbeebf9..e4e3a402a 100644
--- a/shared/uavobjectdefinition/stabilizationsettings.xml
+++ b/shared/uavobjectdefinition/stabilizationsettings.xml
@@ -5,7 +5,7 @@
ModemStat
Autotune
EventDispatcher
+ MagBaro
+ OSDGen
CallbackScheduler0
CallbackScheduler1
CallbackScheduler2
@@ -59,6 +61,8 @@
ModemStat
Autotune
EventDispatcher
+ MagBaro
+ OSDGen
CallbackScheduler0
CallbackScheduler1
CallbackScheduler2
@@ -95,6 +99,8 @@
ModemStat
Autotune
EventDispatcher
+ MagBaro
+ OSDGen
CallbackScheduler0
CallbackScheduler1
CallbackScheduler2