diff --git a/Makefile b/Makefile index 33ea4af40..aef6b7fd1 100644 --- a/Makefile +++ b/Makefile @@ -914,32 +914,32 @@ help: @$(ECHO) @$(ECHO) " [Firmware]" @$(ECHO) " - Build firmware for " - @$(ECHO) " supported boards are ($(ALL_BOARDS))" + @$(ECHO) " Supported boards are ($(ALL_BOARDS))" @$(ECHO) " fw_ - Build firmware for " - @$(ECHO) " supported boards are ($(FW_BOARDS))" + @$(ECHO) " Supported boards are ($(FW_BOARDS))" @$(ECHO) " fw__clean - Remove firmware for " @$(ECHO) " fw__program - Use OpenOCD + JTAG to write firmware to " @$(ECHO) @$(ECHO) " [Bootloader]" @$(ECHO) " bl_ - Build bootloader for " - @$(ECHO) " supported boards are ($(BL_BOARDS))" + @$(ECHO) " Supported boards are ($(BL_BOARDS))" @$(ECHO) " bl__clean - Remove bootloader for " @$(ECHO) " bl__program - Use OpenOCD + JTAG to write bootloader to " @$(ECHO) @$(ECHO) " [Entire Flash]" @$(ECHO) " ef_ - Build entire flash image for " - @$(ECHO) " supported boards are ($(EF_BOARDS))" + @$(ECHO) " Supported boards are ($(EF_BOARDS))" @$(ECHO) " ef__clean - Remove entire flash image for " @$(ECHO) " ef__program - Use OpenOCD + JTAG to write entire flash image to " @$(ECHO) @$(ECHO) " [Bootloader Updater]" @$(ECHO) " bu_ - Build bootloader updater for " - @$(ECHO) " supported boards are ($(BU_BOARDS))" + @$(ECHO) " Supported boards are ($(BU_BOARDS))" @$(ECHO) " bu__clean - Remove bootloader updater for " @$(ECHO) @$(ECHO) " [Unbrick a board]" @$(ECHO) " unbrick_ - Use the STM32's built in boot ROM to write a bootloader to " - @$(ECHO) " supported boards are ($(BL_BOARDS))" + @$(ECHO) " Supported boards are ($(BL_BOARDS))" @$(ECHO) " [Unittests]" @$(ECHO) " ut_ - Build unit test " @$(ECHO) " ut__xml - Run test and capture XML output into a file" @@ -948,14 +948,14 @@ help: @$(ECHO) " [Simulation]" @$(ECHO) " sim_osx - Build OpenPilot simulation firmware for OSX" @$(ECHO) " sim_osx_clean - Delete all build output for the osx simulation" - @$(ECHO) " sim_win32 - Build OpenPilot simulation firmware for" - @$(ECHO) " Windows using mingw and msys" + @$(ECHO) " sim_win32 - Build OpenPilot simulation firmware for Windows" + @$(ECHO) " using mingw and msys" @$(ECHO) " sim_win32_clean - Delete all build output for the win32 simulation" @$(ECHO) @$(ECHO) " [GCS]" @$(ECHO) " gcs - Build the Ground Control System (GCS) application (debug|release)" @$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)" - @$(ECHO) " supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))" + @$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))" @$(ECHO) " gcs_all_clean - Remove the Ground Control System (GCS) application (all build confgurations)" @$(ECHO) @$(ECHO) " [AndroidGCS]" @@ -966,22 +966,35 @@ help: @$(ECHO) @$(ECHO) " [UAVObjects]" @$(ECHO) " uavobjects - Generate source files from the UAVObject definition XML files" - @$(ECHO) " uavobjects_test - parse xml-files - check for valid, duplicate ObjId's, ... " + @$(ECHO) " uavobjects_test - Parse xml-files - check for valid, duplicate ObjId's, ..." @$(ECHO) " uavobjects_ - Generate source files from a subset of the UAVObject definition XML files" - @$(ECHO) " supported groups are ($(UAVOBJ_TARGETS))" + @$(ECHO) " Supported groups are ($(UAVOBJ_TARGETS))" @$(ECHO) @$(ECHO) " [Packaging]" - @$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS" @$(ECHO) " clean_package - Clean, build and package the OpenPilot platform-dependent package" - @$(ECHO) " package - Build and package the OpenPilot platform-dependent package" + @$(ECHO) " package - Build and package the OpenPilot platform-dependent package (no clean)" + @$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS" @$(ECHO) @$(ECHO) " [Code Formatting]" - @$(ECHO) " uncrustify_ - Reformat code. can be flight or ground" + @$(ECHO) " uncrustify_ - Reformat code according to the project's standards" + @$(ECHO) " Supported sources are ($(UNCRUSTIFY_TARGETS))" @$(ECHO) " uncrustify_all - Reformat all source code" @$(ECHO) + @$(ECHO) " [Code Documentation]" + @$(ECHO) " docs_ - Generate HTML documentation for " + @$(ECHO) " Supported sources are ($(DOCS_TARGETS))" + @$(ECHO) " docs_all - Generate HTML documentation for all" + @$(ECHO) " docs__clean - Delete generated documentation for " + @$(ECHO) " docs_all_clean - Delete all generated documentation" + @$(ECHO) @$(ECHO) " Hint: Add V=1 to your command line to see verbose build output." @$(ECHO) - @$(ECHO) " Notes: All tool distribution files will be downloaded into $(DL_DIR)" - @$(ECHO) " All tools will be installed into $(TOOLS_DIR)" - @$(ECHO) " All build output will be placed in $(BUILD_DIR)" + @$(ECHO) " Notes: All tool distribution files will be downloaded into $(DL_DIR)" + @$(ECHO) " All tools will be installed into $(TOOLS_DIR)" + @$(ECHO) " All build output will be placed in $(BUILD_DIR)" + @$(ECHO) + @$(ECHO) " Tool download and install directories can be changed using environment variables:" + @$(ECHO) " OPENPILOT_DL_DIR full path to downloads directory [downloads if not set]" + @$(ECHO) " OPENPILOT_TOOLS_DIR full path to installed tools directory [tools if not set]" + @$(ECHO) " More info: http://wiki.openpilot.org/display/Doc/OpenPilot+Build+System+Overview" @$(ECHO) 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

- ****************************************************************************** - */ + ****************************************************************************** + * @file system_stm32f4xx.c + * @author MCD Application Team + * @version V1.0.0 + * @date 30-September-2011 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F4xx devices, + * and is generated by the clock configuration tool + * stm32f4xx_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f4xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (16 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define + * in "stm32f4xx.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + *============================================================================= + * Supported STM32F4xx device revision | Rev A + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 4 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *----------------------------------------------------------------------------- + * PLL_M | 10 + *----------------------------------------------------------------------------- + * PLL_N | 420 + *----------------------------------------------------------------------------- + * PLL_P | 2 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 5 + *----------------------------------------------------------------------------- + * Prefetch Buffer | OFF + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Enabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /** @addtogroup CMSIS - * @{ - */ + * @{ + */ /** @addtogroup stm32f4xx_system - * @{ - */ - + * @{ + */ + /** @addtogroup STM32F4xx_System_Private_Includes - * @{ - */ + * @{ + */ #include "stm32f4xx.h" /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_TypesDefinitions - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_Defines - * @{ - */ + * @{ + */ /************************* Miscellaneous Configuration ************************/ /*!< Uncomment the following line if you need to use external SRAM mounted - on STM324xG_EVAL board as data memory */ + on STM324xG_EVAL board as data memory */ /* #define DATA_IN_ExtSRAM */ /*!< Uncomment the following line if you need to relocate your vector Table in - Internal SRAM. */ + Internal SRAM. */ /* #define VECT_TAB_SRAM */ #define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ +This value must be a multiple of 0x200. */ /******************************************************************************/ /************************* PLL Parameters *************************************/ /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ -#define PLL_M 4 -#define PLL_N 216 +#define PLL_M 10 +#define PLL_N 420 /* SYSCLK = PLL_VCO / PLL_P */ -#define PLL_P 4 +#define PLL_P 2 /* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ -#define PLL_Q 9 +#define PLL_Q 7 /******************************************************************************/ /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_Macros - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_Variables - * @{ - */ + * @{ + */ - uint32_t SystemCoreClock = 108000000; +uint32_t SystemCoreClock = 168000000; - __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_FunctionPrototypes - * @{ - */ + * @{ + */ static void SetSysClock(void); #ifdef DATA_IN_ExtSRAM - static void SystemInit_ExtMemCtl(void); +static void SystemInit_ExtMemCtl(void); #endif /* DATA_IN_ExtSRAM */ /** - * @} - */ + * @} + */ /** @addtogroup STM32F4xx_System_Private_Functions - * @{ - */ + * @{ + */ /** - * @brief Setup the microcontroller system - * Initialize the Embedded Flash Interface, the PLL and update the - * SystemFrequency variable. - * @param None - * @retval None - */ + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ void SystemInit(void) { - /* FPU settings ------------------------------------------------------------*/ - #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + /* FPU settings ------------------------------------------------------------*/ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ - #endif - /* Reset the RCC clock configuration to the default reset state ------------*/ - /* Set HSION bit */ - RCC->CR |= (uint32_t)0x00000001; - - /* Reset CFGR register */ - RCC->CFGR = 0x00000000; - - /* Reset HSEON, CSSON and PLLON bits */ - RCC->CR &= (uint32_t)0xFEF6FFFF; - - /* Reset PLLCFGR register */ - RCC->PLLCFGR = 0x24003010; - - /* Reset HSEBYP bit */ - RCC->CR &= (uint32_t)0xFFFBFFFF; - - /* Disable all interrupts */ - RCC->CIR = 0x00000000; - +#endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR = 0x00000000; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset PLLCFGR register */ + RCC->PLLCFGR = 0x24003010; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + #ifdef DATA_IN_ExtSRAM - SystemInit_ExtMemCtl(); + SystemInit_ExtMemCtl(); #endif /* DATA_IN_ExtSRAM */ - - /* Configure the System clock source, PLL Multiplier and Divider factors, + + /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ - SetSysClock(); - - /* Configure the Vector Table location add offset address ------------------*/ + SetSysClock(); + + /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif } /** - * @brief Update SystemCoreClock variable according to Clock Register Values. - * The SystemCoreClock variable contains the core clock (HCLK), it can - * be used by the user application to setup the SysTick timer or configure - * other parameters. - * - * @note Each time the core clock (HCLK) changes, this function must be called - * to update SystemCoreClock variable value. Otherwise, any configuration - * based on this variable will be incorrect. - * - * @note - The system frequency computed by this function is not the real - * frequency in the chip. It is calculated based on the predefined - * constant and the selected clock source: - * - * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) - * - * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) - * - * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) - * or HSI_VALUE(*) multiplied/divided by the PLL factors. - * - * (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value - * 16 MHz) but the real value may vary depending on the variations - * in voltage and temperature. - * - * (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value - * 25 MHz), user has to ensure that HSE_VALUE is same as the real - * frequency of the crystal used. Otherwise, this function may - * have wrong result. - * - * - The result of this function could be not correct when using fractional - * value for HSE crystal. - * - * @param None - * @retval None - */ + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value + * 25 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ void SystemCoreClockUpdate(void) { - uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; - - /* Get SYSCLK source -------------------------------------------------------*/ - tmp = RCC->CFGR & RCC_CFGR_SWS; - - switch (tmp) - { - case 0x00: /* HSI used as system clock source */ - SystemCoreClock = HSI_VALUE; - break; - case 0x04: /* HSE used as system clock source */ - SystemCoreClock = HSE_VALUE; - break; - case 0x08: /* PLL used as system clock source */ - - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N - SYSCLK = PLL_VCO / PLL_P - */ - pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; - pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; - - if (pllsource != 0) - { - /* HSE used as PLL clock source */ - pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } - else - { - /* HSI used as PLL clock source */ - pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } - - pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; - SystemCoreClock = pllvco/pllp; - break; - default: - SystemCoreClock = HSI_VALUE; - break; - } - /* Compute HCLK frequency --------------------------------------------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK frequency */ - SystemCoreClock >>= tmp; + uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock source */ + + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + SYSCLK = PLL_VCO / PLL_P + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + else + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; + SystemCoreClock = pllvco/pllp; + break; + default: + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; } /** - * @brief Configures the System clock source, PLL Multiplier and Divider factors, - * AHB/APBx prescalers and Flash settings - * @Note This function should be called only once the RCC clock configuration - * is reset to the default reset state (done in SystemInit() function). - * @param None - * @retval None - */ + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @Note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ static void SetSysClock(void) { -/******************************************************************************/ -/* PLL (clocked by HSE) used as System clock source */ -/******************************************************************************/ - __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - - /* Enable HSE */ - RCC->CR |= ((uint32_t)RCC_CR_HSEON); - - /* Wait till HSE is ready and if Time out is reached exit */ - do - { - HSEStatus = RCC->CR & RCC_CR_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - - if ((RCC->CR & RCC_CR_HSERDY) != RESET) - { - HSEStatus = (uint32_t)0x01; - } - else - { - HSEStatus = (uint32_t)0x00; - } - - if (HSEStatus == (uint32_t)0x01) - { - /* Select regulator voltage output Scale 2 mode, System frequency up to 144 MHz */ - RCC->APB1ENR |= RCC_APB1ENR_PWREN; - PWR->CR &= (uint32_t)~(PWR_CR_VOS); - - /* HCLK = SYSCLK / 1*/ - RCC->CFGR |= RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 2*/ - RCC->CFGR |= RCC_CFGR_PPRE2_DIV2; - - /* PCLK1 = HCLK / 4*/ - RCC->CFGR |= RCC_CFGR_PPRE1_DIV4; - - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); - - /* Enable the main PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_3WS; - - /* Select the main PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= RCC_CFGR_SW_PLL; - - /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); - { - } - } - else - { /* If HSE fails to start-up, the application will have wrong clock - configuration. User can add here some code to deal with this error */ - } - + /******************************************************************************/ + /* PLL (clocked by HSE) used as System clock source */ + /******************************************************************************/ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */ + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + PWR->CR |= PWR_CR_VOS; + + /* HCLK = SYSCLK / 1*/ + RCC->CFGR |= RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV2; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV4; + + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); + + /* Enable the main PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till the main PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; + + /* Select the main PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= RCC_CFGR_SW_PLL; + + /* Wait till the main PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } + } /** - * @brief Setup the external memory controller. Called in startup_stm32f4xx.s - * before jump to __main - * @param None - * @retval None - */ + * @brief Setup the external memory controller. Called in startup_stm32f4xx.s + * before jump to __main + * @param None + * @retval None + */ #ifdef DATA_IN_ExtSRAM /** - * @brief Setup the external memory controller. - * Called in startup_stm32f4xx.s before jump to main. - * This function configures the external SRAM mounted on STM324xG_EVAL board - * This SRAM will be used as program data memory (including heap and stack). - * @param None - * @retval None - */ + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external SRAM mounted on STM324xG_EVAL board + * This SRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ void SystemInit_ExtMemCtl(void) { -/*-- GPIOs Configuration -----------------------------------------------------*/ -/* - +-------------------+--------------------+------------------+------------------+ - + SRAM pins assignment + - +-------------------+--------------------+------------------+------------------+ - | PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 | - | PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 | - | PD4 <-> FSMC_NOE | PE3 <-> FSMC_A19 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 | - | PD5 <-> FSMC_NWE | PE4 <-> FSMC_A20 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 | - | PD8 <-> FSMC_D13 | PE7 <-> FSMC_D4 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 | - | PD9 <-> FSMC_D14 | PE8 <-> FSMC_D5 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 | - | PD10 <-> FSMC_D15 | PE9 <-> FSMC_D6 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 | - | PD11 <-> FSMC_A16 | PE10 <-> FSMC_D7 | PF13 <-> FSMC_A7 |------------------+ - | PD12 <-> FSMC_A17 | PE11 <-> FSMC_D8 | PF14 <-> FSMC_A8 | - | PD13 <-> FSMC_A18 | PE12 <-> FSMC_D9 | PF15 <-> FSMC_A9 | - | PD14 <-> FSMC_D0 | PE13 <-> FSMC_D10 |------------------+ - | PD15 <-> FSMC_D1 | PE14 <-> FSMC_D11 | - | | PE15 <-> FSMC_D12 | - +-------------------+--------------------+ -*/ - /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */ - RCC->AHB1ENR = 0x00000078; - - /* Connect PDx pins to FSMC Alternate function */ - GPIOD->AFR[0] = 0x00cc00cc; - GPIOD->AFR[1] = 0xcc0ccccc; - /* Configure PDx pins in Alternate function mode */ - GPIOD->MODER = 0xaaaa0a0a; - /* Configure PDx pins speed to 100 MHz */ - GPIOD->OSPEEDR = 0xffff0f0f; - /* Configure PDx pins Output type to push-pull */ - GPIOD->OTYPER = 0x00000000; - /* No pull-up, pull-down for PDx pins */ - GPIOD->PUPDR = 0x00000000; - - /* Connect PEx pins to FSMC Alternate function */ - GPIOE->AFR[0] = 0xc00cc0cc; - GPIOE->AFR[1] = 0xcccccccc; - /* Configure PEx pins in Alternate function mode */ - GPIOE->MODER = 0xaaaa828a; - /* Configure PEx pins speed to 100 MHz */ - GPIOE->OSPEEDR = 0xffffc3cf; - /* Configure PEx pins Output type to push-pull */ - GPIOE->OTYPER = 0x00000000; - /* No pull-up, pull-down for PEx pins */ - GPIOE->PUPDR = 0x00000000; - - /* Connect PFx pins to FSMC Alternate function */ - GPIOF->AFR[0] = 0x00cccccc; - GPIOF->AFR[1] = 0xcccc0000; - /* Configure PFx pins in Alternate function mode */ - GPIOF->MODER = 0xaa000aaa; - /* Configure PFx pins speed to 100 MHz */ - GPIOF->OSPEEDR = 0xff000fff; - /* Configure PFx pins Output type to push-pull */ - GPIOF->OTYPER = 0x00000000; - /* No pull-up, pull-down for PFx pins */ - GPIOF->PUPDR = 0x00000000; - - /* Connect PGx pins to FSMC Alternate function */ - GPIOG->AFR[0] = 0x00cccccc; - GPIOG->AFR[1] = 0x000000c0; - /* Configure PGx pins in Alternate function mode */ - GPIOG->MODER = 0x00080aaa; - /* Configure PGx pins speed to 100 MHz */ - GPIOG->OSPEEDR = 0x000c0fff; - /* Configure PGx pins Output type to push-pull */ - GPIOG->OTYPER = 0x00000000; - /* No pull-up, pull-down for PGx pins */ - GPIOG->PUPDR = 0x00000000; - -/*-- FSMC Configuration ------------------------------------------------------*/ - /* Enable the FSMC interface clock */ - RCC->AHB3ENR = 0x00000001; - - /* Configure and enable Bank1_SRAM2 */ - FSMC_Bank1->BTCR[2] = 0x00001015; - FSMC_Bank1->BTCR[3] = 0x00010603; - FSMC_Bank1E->BWTR[2] = 0x0fffffff; - /* - Bank1_SRAM2 is configured as follow: - - p.FSMC_AddressSetupTime = 3; - p.FSMC_AddressHoldTime = 0; - p.FSMC_DataSetupTime = 6; - p.FSMC_BusTurnAroundDuration = 1; - p.FSMC_CLKDivision = 0; - p.FSMC_DataLatency = 0; - p.FSMC_AccessMode = FSMC_AccessMode_A; - - FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2; - FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable; - FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_PSRAM; - FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b; - FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable; - FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable; - FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low; - FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable; - FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState; - FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable; - FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable; - FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable; - FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable; - FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p; - FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p; -*/ + /*-- GPIOs Configuration -----------------------------------------------------*/ + /* + +-------------------+--------------------+------------------+------------------+ + + SRAM pins assignment + + +-------------------+--------------------+------------------+------------------+ + | PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 | + | PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 | + | PD4 <-> FSMC_NOE | PE3 <-> FSMC_A19 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 | + | PD5 <-> FSMC_NWE | PE4 <-> FSMC_A20 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 | + | PD8 <-> FSMC_D13 | PE7 <-> FSMC_D4 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 | + | PD9 <-> FSMC_D14 | PE8 <-> FSMC_D5 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 | + | PD10 <-> FSMC_D15 | PE9 <-> FSMC_D6 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 | + | PD11 <-> FSMC_A16 | PE10 <-> FSMC_D7 | PF13 <-> FSMC_A7 |------------------+ + | PD12 <-> FSMC_A17 | PE11 <-> FSMC_D8 | PF14 <-> FSMC_A8 | + | PD13 <-> FSMC_A18 | PE12 <-> FSMC_D9 | PF15 <-> FSMC_A9 | + | PD14 <-> FSMC_D0 | PE13 <-> FSMC_D10 |------------------+ + | PD15 <-> FSMC_D1 | PE14 <-> FSMC_D11 | + | | PE15 <-> FSMC_D12 | + +-------------------+--------------------+ + */ + /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */ + RCC->AHB1ENR = 0x00000078; + + /* Connect PDx pins to FSMC Alternate function */ + GPIOD->AFR[0] = 0x00cc00cc; + GPIOD->AFR[1] = 0xcc0ccccc; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xaaaa0a0a; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xffff0f0f; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FSMC Alternate function */ + GPIOE->AFR[0] = 0xc00cc0cc; + GPIOE->AFR[1] = 0xcccccccc; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xaaaa828a; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xffffc3cf; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FSMC Alternate function */ + GPIOF->AFR[0] = 0x00cccccc; + GPIOF->AFR[1] = 0xcccc0000; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xaa000aaa; + /* Configure PFx pins speed to 100 MHz */ + GPIOF->OSPEEDR = 0xff000fff; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FSMC Alternate function */ + GPIOG->AFR[0] = 0x00cccccc; + GPIOG->AFR[1] = 0x000000c0; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0x00080aaa; + /* Configure PGx pins speed to 100 MHz */ + GPIOG->OSPEEDR = 0x000c0fff; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + + /*-- FSMC Configuration ------------------------------------------------------*/ + /* Enable the FSMC interface clock */ + RCC->AHB3ENR = 0x00000001; + + /* Configure and enable Bank1_SRAM2 */ + FSMC_Bank1->BTCR[2] = 0x00001015; + FSMC_Bank1->BTCR[3] = 0x00010603; + FSMC_Bank1E->BWTR[2] = 0x0fffffff; + /* + Bank1_SRAM2 is configured as follow: + + p.FSMC_AddressSetupTime = 3; + p.FSMC_AddressHoldTime = 0; + p.FSMC_DataSetupTime = 6; + p.FSMC_BusTurnAroundDuration = 1; + p.FSMC_CLKDivision = 0; + p.FSMC_DataLatency = 0; + p.FSMC_AccessMode = FSMC_AccessMode_A; + + FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2; + FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable; + FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_PSRAM; + FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b; + FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low; + FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState; + FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable; + FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable; + FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p; + FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p; + */ + } #endif /* DATA_IN_ExtSRAM */ /** - * @} - */ + * @} + */ /** - * @} - */ - + * @} + */ + /** - * @} - */ + * @} + */ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ - diff --git a/flight/pios/stm32f4xx/pios_sys.c b/flight/pios/stm32f4xx/pios_sys.c index 4620301a9..7819513a3 100644 --- a/flight/pios/stm32f4xx/pios_sys.c +++ b/flight/pios/stm32f4xx/pios_sys.c @@ -43,184 +43,184 @@ void SysTick_Handler(void); #define MEM32(addr) (*((volatile uint32_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { - /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ - SystemInit(); - SystemCoreClockUpdate(); /* update SystemCoreClock for use elsewhere */ + /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ + SystemInit(); + SystemCoreClockUpdate(); /* update SystemCoreClock for use elsewhere */ - /* - * @todo might make sense to fetch the bus clocks and save them somewhere to avoid - * having to use the clunky get-all-clocks API everytime we need one. - */ + /* + * @todo might make sense to fetch the bus clocks and save them somewhere to avoid + * having to use the clunky get-all-clocks API everytime we need one. + */ - /* Initialise Basic NVIC */ - /* do this early to ensure that we take exceptions in the right place */ - NVIC_Configuration(); + /* Initialise Basic NVIC */ + /* do this early to ensure that we take exceptions in the right place */ + NVIC_Configuration(); - /* Init the delay system */ - PIOS_DELAY_Init(); + /* Init the delay system */ + PIOS_DELAY_Init(); - /* - * Turn on all the peripheral clocks. - * Micromanaging clocks makes no sense given the power situation in the system, so - * light up everything we might reasonably use here and just leave it on. - */ - RCC_AHB1PeriphClockCmd( - RCC_AHB1Periph_GPIOA | - RCC_AHB1Periph_GPIOB | - RCC_AHB1Periph_GPIOC | - RCC_AHB1Periph_GPIOD | - RCC_AHB1Periph_GPIOE | - RCC_AHB1Periph_GPIOF | - RCC_AHB1Periph_GPIOG | - RCC_AHB1Periph_GPIOH | - RCC_AHB1Periph_GPIOI | - RCC_AHB1Periph_CRC | - RCC_AHB1Periph_FLITF | - RCC_AHB1Periph_SRAM1 | - RCC_AHB1Periph_SRAM2 | - RCC_AHB1Periph_BKPSRAM | - RCC_AHB1Periph_DMA1 | - RCC_AHB1Periph_DMA2 | - //RCC_AHB1Periph_ETH_MAC | No ethernet - //RCC_AHB1Periph_ETH_MAC_Tx | - //RCC_AHB1Periph_ETH_MAC_Rx | - //RCC_AHB1Periph_ETH_MAC_PTP | - //RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) - //RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) - 0, ENABLE); - RCC_AHB2PeriphClockCmd( - //RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? - //RCC_AHB2Periph_CRYP | No crypto - //RCC_AHB2Periph_HASH | No hash generator - //RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired - //RCC_AHB2Periph_OTG_FS | - 0, ENABLE); - RCC_AHB3PeriphClockCmd( - //RCC_AHB3Periph_FSMC | No external static memory - 0, ENABLE); - RCC_APB1PeriphClockCmd( - RCC_APB1Periph_TIM2 | - RCC_APB1Periph_TIM3 | - RCC_APB1Periph_TIM4 | - RCC_APB1Periph_TIM5 | - RCC_APB1Periph_TIM6 | - RCC_APB1Periph_TIM7 | - RCC_APB1Periph_TIM12 | - RCC_APB1Periph_TIM13 | - RCC_APB1Periph_TIM14 | - RCC_APB1Periph_WWDG | - RCC_APB1Periph_SPI2 | - RCC_APB1Periph_SPI3 | - RCC_APB1Periph_USART2 | - RCC_APB1Periph_USART3 | - RCC_APB1Periph_UART4 | - RCC_APB1Periph_UART5 | - RCC_APB1Periph_I2C1 | - RCC_APB1Periph_I2C2 | - RCC_APB1Periph_I2C3 | - RCC_APB1Periph_CAN1 | - RCC_APB1Periph_CAN2 | - RCC_APB1Periph_PWR | - RCC_APB1Periph_DAC | - 0, ENABLE); + /* + * Turn on all the peripheral clocks. + * Micromanaging clocks makes no sense given the power situation in the system, so + * light up everything we might reasonably use here and just leave it on. + */ + RCC_AHB1PeriphClockCmd( + RCC_AHB1Periph_GPIOA | + RCC_AHB1Periph_GPIOB | + RCC_AHB1Periph_GPIOC | + RCC_AHB1Periph_GPIOD | + RCC_AHB1Periph_GPIOE | + RCC_AHB1Periph_GPIOF | + RCC_AHB1Periph_GPIOG | + RCC_AHB1Periph_GPIOH | + RCC_AHB1Periph_GPIOI | + RCC_AHB1Periph_CRC | + RCC_AHB1Periph_FLITF | + RCC_AHB1Periph_SRAM1 | + RCC_AHB1Periph_SRAM2 | + RCC_AHB1Periph_BKPSRAM | + RCC_AHB1Periph_DMA1 | + RCC_AHB1Periph_DMA2 | + //RCC_AHB1Periph_ETH_MAC | No ethernet + //RCC_AHB1Periph_ETH_MAC_Tx | + //RCC_AHB1Periph_ETH_MAC_Rx | + //RCC_AHB1Periph_ETH_MAC_PTP | + //RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) + //RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) + 0, ENABLE); + RCC_AHB2PeriphClockCmd( + //RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? + //RCC_AHB2Periph_CRYP | No crypto + //RCC_AHB2Periph_HASH | No hash generator + //RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired + //RCC_AHB2Periph_OTG_FS | + 0, ENABLE); + RCC_AHB3PeriphClockCmd( + //RCC_AHB3Periph_FSMC | No external static memory + 0, ENABLE); + RCC_APB1PeriphClockCmd( + RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM5 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + RCC_APB1Periph_TIM12 | + RCC_APB1Periph_TIM13 | + RCC_APB1Periph_TIM14 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_SPI2 | + RCC_APB1Periph_SPI3 | + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_UART4 | + RCC_APB1Periph_UART5 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_I2C3 | + RCC_APB1Periph_CAN1 | + RCC_APB1Periph_CAN2 | + RCC_APB1Periph_PWR | + RCC_APB1Periph_DAC | + 0, ENABLE); - RCC_APB2PeriphClockCmd( - RCC_APB2Periph_TIM1 | - RCC_APB2Periph_TIM8 | - RCC_APB2Periph_USART1 | - RCC_APB2Periph_USART6 | - RCC_APB2Periph_ADC | - RCC_APB2Periph_ADC1 | - RCC_APB2Periph_ADC2 | - RCC_APB2Periph_ADC3 | - RCC_APB2Periph_SDIO | - RCC_APB2Periph_SPI1 | - RCC_APB2Periph_SYSCFG | - RCC_APB2Periph_TIM9 | - RCC_APB2Periph_TIM10 | - RCC_APB2Periph_TIM11 | - 0, ENABLE); + RCC_APB2PeriphClockCmd( + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_USART6 | + RCC_APB2Periph_ADC | + RCC_APB2Periph_ADC1 | + RCC_APB2Periph_ADC2 | + RCC_APB2Periph_ADC3 | + RCC_APB2Periph_SDIO | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_SYSCFG | + RCC_APB2Periph_TIM9 | + RCC_APB2Periph_TIM10 | + RCC_APB2Periph_TIM11 | + 0, ENABLE); - /* - * Configure all pins as input / pullup to avoid issues with - * uncommitted pins, excepting special-function pins that we need to - * remain as-is. - */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input + /* + * Configure all pins as input / pullup to avoid issues with + * uncommitted pins, excepting special-function pins that we need to + * remain as-is. + */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; #if (PIOS_USB_ENABLED) - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone #endif - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone - GPIO_Init(GPIOA, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone + GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4); // leave JTAG pins alone - GPIO_Init(GPIOB, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);// leave JTAG pins alone + GPIO_Init(GPIOB, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; - GPIO_Init(GPIOC, &GPIO_InitStructure); - GPIO_Init(GPIOD, &GPIO_InitStructure); - GPIO_Init(GPIOE, &GPIO_InitStructure); - GPIO_Init(GPIOF, &GPIO_InitStructure); - GPIO_Init(GPIOG, &GPIO_InitStructure); - GPIO_Init(GPIOH, &GPIO_InitStructure); - GPIO_Init(GPIOI, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_Init(GPIOC, &GPIO_InitStructure); + GPIO_Init(GPIOD, &GPIO_InitStructure); + GPIO_Init(GPIOE, &GPIO_InitStructure); + GPIO_Init(GPIOF, &GPIO_InitStructure); + GPIO_Init(GPIOG, &GPIO_InitStructure); + GPIO_Init(GPIOH, &GPIO_InitStructure); + GPIO_Init(GPIOI, &GPIO_InitStructure); } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • 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 d2de7fb35..4ea6bbae8 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -967,6 +967,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 aa50afed6..ef1434754 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 @@ - + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 5ee22c43e..08a62c5b8 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,19 +2,19 @@ Selection of optional hardware configurations. - - + + - - + + - - + + @@ -26,7 +26,7 @@ - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index a15b9039f..c8486ce15 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -43,32 +43,32 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold" limits="\ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\ + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\ + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\ + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\ + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\ + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner"/> + %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/> diff --git a/shared/uavobjectdefinition/osdsettings.xml b/shared/uavobjectdefinition/osdsettings.xml index 7575f6ed2..c3ceef706 100644 --- a/shared/uavobjectdefinition/osdsettings.xml +++ b/shared/uavobjectdefinition/osdsettings.xml @@ -14,6 +14,9 @@ + + + diff --git a/shared/uavobjectdefinition/poilearnsettings.xml b/shared/uavobjectdefinition/poilearnsettings.xml new file mode 100644 index 000000000..281d064e3 --- /dev/null +++ b/shared/uavobjectdefinition/poilearnsettings.xml @@ -0,0 +1,10 @@ + + + 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 @@ - + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index bd4104f9e..869e3b84b 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -27,6 +27,8 @@ 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