diff --git a/Makefile b/Makefile index cdeafcb72..a2bcd4cb7 100644 --- a/Makefile +++ b/Makefile @@ -200,10 +200,9 @@ export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics DIRS += $(OPGCSSYNTHDIR) # Define supported board lists -ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum +ALL_BOARDS := oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum # Short names of each board (used to display board name in parallel builds) -coptercontrol_short := 'cc ' oplinkmini_short := 'oplm' revolution_short := 'revo' osd_short := 'osd ' diff --git a/WHATSNEW.txt b/WHATSNEW.txt index 79c46fdae..27c13926b 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,4 +1,4 @@ ---- RELEASE-15.02.02-RC3 --- +--- RELEASE-15.02.02 --- This release fixes a bug that prevents revo onboard mag to work correctly. Release Notes - OpenPilot - Version RELEASE-15.02.02 @@ -11,11 +11,6 @@ https://progress.openpilot.org/issues/?filter=12262 * [OP-1821] - Tricopter tail servo wrong speed on wizard * [OP-1827] - Version ID wrong in Windows uninstaller * [OP-1857] - PPM on Flexi does not work on CC/CC3D - * [OP-1872] - Vehicle Wiz Tricopter tail servo settings don't save - -** Improvement - * [OP-1835] - Add motor constraints in place of overhead throttle buffer for enhanced stability and power. Add swash constraints for helis. - ** Task * [OP-1831] - due to oneshot higher pid values ki now shows "red" warning in stabilization page diff --git a/flight/modules/PathFollower/pidcontroldown.cpp b/flight/libraries/pid/pidcontroldown.cpp similarity index 96% rename from flight/modules/PathFollower/pidcontroldown.cpp rename to flight/libraries/pid/pidcontroldown.cpp index ea01b7e06..7f06eac93 100644 --- a/flight/modules/PathFollower/pidcontroldown.cpp +++ b/flight/libraries/pid/pidcontroldown.cpp @@ -45,7 +45,6 @@ extern "C" { #include } -#include "pathfollowerfsm.h" #include "pidcontroldown.h" #define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f @@ -59,17 +58,17 @@ extern "C" { PIDControlDown::PIDControlDown() : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f), - mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), - mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false) + mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), + mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true) { Deactivate(); } PIDControlDown::~PIDControlDown() {} -void PIDControlDown::Initialize(PathFollowerFSM *fsm) +void PIDControlDown::Initialize(PIDControlDownCallback *callback) { - mFSM = fsm; + mCallback = callback; } void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust) @@ -80,7 +79,6 @@ void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust) void PIDControlDown::Deactivate() { - // pid_zero(&PID); mActive = false; } @@ -333,9 +331,9 @@ void PIDControlDown::UpdateVelocityState(float pv) { mVelocityState = pv; - if (mFSM) { + if (mCallback) { // The FSM controls the actual descent velocity and introduces step changes as required - float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget); + float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget); // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f ); mVelocitySetpointCurrent = velocitySetpointDesired; } else { @@ -354,8 +352,8 @@ float PIDControlDown::GetDownCommand(void) float ulow = mMinThrust; float uhigh = mMaxThrust; - if (mFSM) { - mFSM->BoundThrust(ulow, uhigh); + if (mCallback) { + mCallback->BoundThrust(ulow, uhigh); } float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh); pidStatus.setpoint = mVelocitySetpointCurrent; diff --git a/flight/modules/PathFollower/inc/pidcontroldown.h b/flight/libraries/pid/pidcontroldown.h similarity index 88% rename from flight/modules/PathFollower/inc/pidcontroldown.h rename to flight/libraries/pid/pidcontroldown.h index 9546e2e75..8ea185610 100644 --- a/flight/modules/PathFollower/inc/pidcontroldown.h +++ b/flight/libraries/pid/pidcontroldown.h @@ -34,16 +34,20 @@ extern "C" { #include #include } -#include "pathfollowerfsm.h" +#include "pidcontroldowncallback.h" class PIDControlDown { public: PIDControlDown(); ~PIDControlDown(); - void Initialize(PathFollowerFSM *fsm); + void Initialize(PIDControlDownCallback *callback); void SetThrustLimits(float min_thrust, float max_thrust); void Deactivate(); void Activate(); + bool IsActive() + { + return mActive; + } void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax); void UpdateNeutralThrust(float neutral); void UpdateVelocitySetpoint(float setpoint); @@ -58,6 +62,14 @@ public: void ControlPositionWithPath(struct path_status *progress); void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate); + void DisableNeutralThrustCalc() + { + mAllowNeutralThrustCalc = false; + } + void EnableNeutralThrustCalc() + { + mAllowNeutralThrustCalc = true; + } private: void setup_neutralThrustCalc(); @@ -69,7 +81,7 @@ private: float mVelocitySetpointCurrent; float mVelocityState; float mDownCommand; - PathFollowerFSM *mFSM; + PIDControlDownCallback *mCallback; float mNeutral; float mVelocityMax; struct pid PIDpos; @@ -77,7 +89,6 @@ private: float mPositionState; float mMinThrust; float mMaxThrust; - uint8_t mActive; struct NeutralThrustEstimation { uint32_t count; @@ -90,6 +101,8 @@ private: bool have_correction; }; struct NeutralThrustEstimation neutralThrustEst; + bool mActive; + bool mAllowNeutralThrustCalc; }; #endif // PIDCONTROLDOWN_H diff --git a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h b/flight/libraries/pid/pidcontroldowncallback.h similarity index 61% rename from flight/targets/boards/coptercontrol/firmware/inc/openpilot.h rename to flight/libraries/pid/pidcontroldowncallback.h index 5576baca6..b6aedf8dd 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h +++ b/flight/libraries/pid/pidcontroldowncallback.h @@ -1,12 +1,15 @@ /** ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System + * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup OpenPilotCore OpenPilot Core + * @addtogroup PID Library + * @brief Thrust control callback pure virtual * @{ - * @file openpilot.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main OpenPilot header. + * + * @file pidcontroldowncallback.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Interface class for PathFollower FSMs + * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -25,28 +28,15 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#ifndef PIDCONTROLDOWNCALLBACK_H +#define PIDCONTROLDOWNCALLBACK_H -#ifndef OPENPILOT_H -#define OPENPILOT_H +class PIDControlDownCallback { +public: + // PIDControlDownCalback() {}; + virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0; + virtual float BoundVelocityDown(float velocity) = 0; + // virtual ~PIDControlDownCalback() = 0; +}; -/* PIOS Includes */ -#include - -/* OpenPilot Libraries */ -#include -#include -#include -#include - -#include "alarms.h" -#include - -/* Global Functions */ -void OpenPilotInit(void); - -#endif /* OPENPILOT_H */ - -/** - * @} - * @} - */ +#endif // PIDCONTROLDOWNCALLBACK_H diff --git a/flight/modules/Airspeed/baro_airspeed_mpxv.c b/flight/modules/Airspeed/baro_airspeed_mpxv.c index 086159e49..5c7ead777 100644 --- a/flight/modules/Airspeed/baro_airspeed_mpxv.c +++ b/flight/modules/Airspeed/baro_airspeed_mpxv.c @@ -46,7 +46,7 @@ #define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] #define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] -#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 // low pass filter +#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 256 // low pass filter // Private types @@ -104,9 +104,10 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa } } sensor.zeroPoint = airspeedSettings->ZeroPoint; + sensor.scale = airspeedSettings->Scale; - // Filter CAS - float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter. + // Filter CAS : airspeedSettings->SamplePeriod value is 0 - 255 range + float alpha = 1 - (airspeedSettings->SamplePeriod / ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter. airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index ad3cad4ce..400b921ac 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -43,6 +43,7 @@ static void com2UsbBridgeTask(void *parameters); static void usb2ComBridgeTask(void *parameters); static void updateSettings(UAVObjEvent *ev); +static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); // **************** // Private constants @@ -95,8 +96,16 @@ static int32_t comUsbBridgeStart(void) static int32_t comUsbBridgeInitialize(void) { // TODO: Get from settings object - usart_port = PIOS_COM_BRIDGE; - vcp_port = PIOS_COM_VCP; + usart_port = PIOS_COM_BRIDGE; + vcp_port = PIOS_COM_VCP; + + // Register the call back handler for USB control line changes to simply + // pass these onto any handler registered on the USART + if (vcp_port) { + PIOS_COM_RegisterCtrlLineCallback(vcp_port, + usb2ComBridgeSetCtrlLine, + usart_port); + } #ifdef MODULE_COMUSBBRIDGE_BUILTIN bridge_enabled = true; @@ -168,6 +177,14 @@ static void usb2ComBridgeTask(__attribute__((unused)) void *parameters) } } +/* This routine is registered with the USB driver and will be called in the + * event of a control line state change. It will then call down to the USART + * driver to drive the required control line state. + */ +static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state) +{ + PIOS_COM_SetCtrlLine(com_id, mask, state); +} static void updateSettings(__attribute__((unused)) UAVObjEvent *ev) { diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index d9a080256..28c586b1e 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -50,6 +50,7 @@ #include "UBX.h" #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #include "inc/ubx_autoconfig.h" +// #include "../../libraries/inc/fifo_buffer.h" #endif #include @@ -60,7 +61,7 @@ PERF_DEFINE_COUNTER(counterParse); // Private functions static void gpsTask(void *parameters); -static void updateHwSettings(); +static void updateHwSettings(UAVObjEvent *ev); #ifdef PIOS_GPS_SETS_HOMELOCATION static void setHomeLocation(GPSPositionSensorData *gpsData); @@ -111,6 +112,8 @@ void updateGpsSettings(UAVObjEvent *ev); // **************** // Private variables +static GPSSettingsData gpsSettings; + static uint32_t gpsPort; static bool gpsEnabled = false; @@ -150,15 +153,15 @@ int32_t GPSStart(void) * \return -1 if initialisation failed * \return 0 on success */ + int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; - GPSSettingsDataProtocolOptions gpsProtocol; + HwSettingsInitialize(); #ifdef MODULE_GPS_BUILTIN gpsEnabled = true; #else - HwSettingsInitialize(); HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); @@ -188,8 +191,12 @@ int32_t GPSInitialize(void) AuxMagSettingsUpdatedCb(NULL); AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif - updateHwSettings(); -#else + GPSSettingsInitialize(); + // updateHwSettings() uses gpsSettings + GPSSettingsGet(&gpsSettings); + // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running + updateHwSettings(0); +#else /* if defined(REVOLUTION) */ if (gpsPort && gpsEnabled) { GPSPositionSensorInitialize(); GPSVelocitySensorInitialize(); @@ -200,27 +207,29 @@ int32_t GPSInitialize(void) #if defined(PIOS_GPS_SETS_HOMELOCATION) HomeLocationInitialize(); #endif - updateHwSettings(); + GPSSettingsInitialize(); + // updateHwSettings() uses gpsSettings + GPSSettingsGet(&gpsSettings); + // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running + updateHwSettings(0); } #endif /* if defined(REVOLUTION) */ if (gpsPort && gpsEnabled) { - GPSSettingsInitialize(); - GPSSettingsDataProtocolGet(&gpsProtocol); - switch (gpsProtocol) { -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case GPSSETTINGS_DATAPROTOCOL_NMEA: - gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH); - break; +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER) + gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH); +#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER) + gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); +#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH); +#else + gps_rx_buffer = NULL; #endif - case GPSSETTINGS_DATAPROTOCOL_UBX: - gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); - break; - default: - gps_rx_buffer = NULL; - } +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) PIOS_Assert(gps_rx_buffer); +#endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup GPSSettingsConnectCallback(updateGpsSettings); #endif return 0; @@ -245,15 +254,13 @@ static void gpsTask(__attribute__((unused)) void *parameters) portTickType homelocationSetDelay = 0; #endif GPSPositionSensorData gpspositionsensor; - GPSSettingsData gpsSettings; - - GPSSettingsGet(&gpsSettings); timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + // this should be done in the task because it calls out to actually start the GPS serial reads updateGpsSettings(0); #endif @@ -270,9 +277,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; - uint8_t status; - GPSPositionSensorStatusGet(&status); - ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS); + + ubx_autoconfig_run(&buffer, &count); // Something to send? if (count) { // clear ack/nak @@ -284,10 +290,28 @@ static void gpsTask(__attribute__((unused)) void *parameters) PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); } } -#endif - // This blocks the task until there is something on the buffer + + // need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct + int32_t ac_status = ubx_autoconfig_get_status(); + static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED + + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING + + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE + + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR; + gpspositionsensor.AutoConfigStatus = + ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED : + ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE : + ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR : + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING; + if (gpspositionsensor.AutoConfigStatus != lastStatus) { + GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); + lastStatus = gpspositionsensor.AutoConfigStatus; + } +#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ + + // This blocks the task until there is something on the buffer (or 100ms? passes) uint16_t cnt; - while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) { + cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay); + if (cnt > 0) { PERF_TIMED_SECTION_START(counterParse); PERF_TRACK_VALUE(counterBytesIn, cnt); PERF_MEASURE_PERIOD(counterRate); @@ -300,24 +324,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) case GPSSETTINGS_DATAPROTOCOL_UBX: - { -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - int32_t ac_status = ubx_autoconfig_get_status(); - static uint8_t lastStatus = 0; - - gpspositionsensor.AutoConfigStatus = - ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED : - ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE : - ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR : - GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING; - if (gpspositionsensor.AutoConfigStatus != lastStatus) { - GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); - lastStatus = gpspositionsensor.AutoConfigStatus; - } -#endif res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); - } - break; + break; #endif default: res = NO_PARSER; // this should not happen @@ -435,40 +443,136 @@ static void setHomeLocation(GPSPositionSensorData *gpsData) * like protocol, etc. Thus the HwSettings object which contains the * GPS port speed is used for now. */ -static void updateHwSettings() -{ - if (gpsPort) { - // Retrieve settings - HwSettingsGPSSpeedOptions speed; - HwSettingsGPSSpeedGet(&speed); - // Set port speed - switch (speed) { - case HWSETTINGS_GPSSPEED_2400: - PIOS_COM_ChangeBaud(gpsPort, 2400); - break; - case HWSETTINGS_GPSSPEED_4800: - PIOS_COM_ChangeBaud(gpsPort, 4800); - break; - case HWSETTINGS_GPSSPEED_9600: - PIOS_COM_ChangeBaud(gpsPort, 9600); - break; - case HWSETTINGS_GPSSPEED_19200: - PIOS_COM_ChangeBaud(gpsPort, 19200); - break; - case HWSETTINGS_GPSSPEED_38400: - PIOS_COM_ChangeBaud(gpsPort, 38400); - break; - case HWSETTINGS_GPSSPEED_57600: - PIOS_COM_ChangeBaud(gpsPort, 57600); - break; - case HWSETTINGS_GPSSPEED_115200: - PIOS_COM_ChangeBaud(gpsPort, 115200); - break; - case HWSETTINGS_GPSSPEED_230400: - PIOS_COM_ChangeBaud(gpsPort, 230400); - break; +// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running +static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) +{ + // with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program) + // no booting of Revo should be required during any setup or testing, just Send the settings you want to play with + // with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port + // with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it + // GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is + // connected, even to an incorrectly configured (e.g. factory default) GPS + + // Use cases: + // - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled + // - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore + // - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes + // - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too + // - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running + // - the user can plug in a correctly configured GPS and use autoconfig-disabled + // - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong) + // - - disable UBX GPS autoconfig + // - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate + // - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6) + // - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data + // - - - (way more than default) at 4800 baud or slower + // - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings + // - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port) + // - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE" + // - - enable autoconfig-store and it will save the new baud rate and the correct message configuration + // - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE" + // - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled + // - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting + // - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug + // - - or with this code, OP could and even should just ship GPS's with default settings + + // if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store + // - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore) + // - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate? + + // About using UBlox GPS's with default settings (9600 baud and NEMA data): + // - the default uBlox settings (different than OP settings) are NEMA and 9600 baud + // - - and that is OK and you use autoconfig-nostore + // - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings, + // - - you get them automatically changed to match whatever version of OP code you are running + // - - but 9600 is only OK for this autoconfig startup + // - by my testing, the 9600 initial to 57600 final baud startup takes: + // - - 0:10 if the GPS has been configured to send OP data at 9600 + // - - 0:06 if the GPS has default data settings (NEMA) at 9600 + // - - reminder that even 0:10 isn't that bad. You need to wait for the GPS to acquire satellites, + + // Some things you want to think about if you want to play around with this: + // - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure + // - - at 2400 it could take a long time to autoconfig or error out + // - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed + // - if you autoconfigure an OP configuration at 9600 baud or lower + // - - it will actually make a factory default configuration (not an OP configuration) at that baud rate + // - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync + // - - rebooting the Revo from the Ground Control Station (without powering the GPS down too) + // - - can leave the baud rates out of sync if you are using autoconfig + // - - just power off and on both the Revo and the GPS + // - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400 + // - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled + // - - I haven't tested other GPS's in regard to this + + // since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset + // - if set to 9600 baud (or lower) + + if (gpsPort) { + HwSettingsGPSSpeedOptions speed; + +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + // use 9600 baud during startup if autoconfig is enabled + // that is the flag that the code should assumes a factory default baud rate + if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) { + speed = HWSETTINGS_GPSSPEED_9600; + } else +#endif + { + // Retrieve settings + HwSettingsGPSSpeedGet(&speed); } + + // must always set the baud here, even if it looks like it is the same + // e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here + + // on startup (ev == NULL) we must set the Revo port baud rate + // after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled + // always we must set the Revo port baud rate if not using UBX +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) +#endif + { + // Set Revo port speed + switch (speed) { + case HWSETTINGS_GPSSPEED_2400: + PIOS_COM_ChangeBaud(gpsPort, 2400); + break; + case HWSETTINGS_GPSSPEED_4800: + PIOS_COM_ChangeBaud(gpsPort, 4800); + break; + case HWSETTINGS_GPSSPEED_9600: + PIOS_COM_ChangeBaud(gpsPort, 9600); + break; + case HWSETTINGS_GPSSPEED_19200: + PIOS_COM_ChangeBaud(gpsPort, 19200); + break; + case HWSETTINGS_GPSSPEED_38400: + PIOS_COM_ChangeBaud(gpsPort, 38400); + break; + case HWSETTINGS_GPSSPEED_57600: + PIOS_COM_ChangeBaud(gpsPort, 57600); + break; + case HWSETTINGS_GPSSPEED_115200: + PIOS_COM_ChangeBaud(gpsPort, 115200); + break; + case HWSETTINGS_GPSSPEED_230400: + PIOS_COM_ChangeBaud(gpsPort, 230400); + break; + } +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + // even changing the baud rate will make it re-verify the sensor type + // that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct + ubx_reset_sensor_type(); +#endif + } +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + else { + // it will never do this during startup because ev == NULL + ubx_autoconfig_set(NULL); + } +#endif } } @@ -480,32 +584,26 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { - uint8_t ubxAutoConfig; - uint8_t ubxDynamicModel; - uint8_t ubxSbasMode; ubx_autoconfig_settings_t newconfig; - uint8_t ubxSbasSats; - uint8_t ubxGnssMode; - GPSSettingsUbxRateGet(&newconfig.navRate); + GPSSettingsGet(&gpsSettings); - GPSSettingsUbxAutoConfigGet(&ubxAutoConfig); - newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true; - newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE; + newconfig.navRate = gpsSettings.UbxRate; - GPSSettingsUbxDynamicModelGet(&ubxDynamicModel); - newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : + newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true; + newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE; + + newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : UBX_DYNMODEL_AIRBORNE1G; - GPSSettingsUbxSBASModeGet(&ubxSbasMode); - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION: case GPSSETTINGS_UBXSBASMODE_CORRECTION: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY: @@ -516,7 +614,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASCorrection = false; } - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_RANGING: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION: case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY: @@ -527,7 +625,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASRanging = false; } - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_INTEGRITY: case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY: @@ -538,19 +636,15 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASIntegrity = false; } - GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed); + newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed; - GPSSettingsUbxSBASSatsGet(&ubxSbasSats); + newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN; - newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN; - - GPSSettingsUbxGNSSModeGet(&ubxGnssMode); - - switch (ubxGnssMode) { + switch (gpsSettings.UbxGNSSMode) { case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS: newconfig.enableGPS = true; newconfig.enableGLONASS = true; @@ -583,7 +677,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) break; } - ubx_autoconfig_set(newconfig); + ubx_autoconfig_set(&newconfig); } #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ /** diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index a75efd28a..2fc61a283 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -43,7 +43,7 @@ static bool useMag = false; #endif -static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; +GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; static bool usePvt = false; static uint32_t lastPvtTime = 0; @@ -466,9 +466,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver; ubxHwVersion = atoi(mon_ver->hwVersion); - sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); + // send sensor type right now because on UBX NEMA we don't get a full set of messages + // and we want to be able to see sensor type even on UBX NEMA GPS's + GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); } static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index a7e938f46..c4ea60357 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -102,7 +102,8 @@ typedef enum { UBX_ID_CFG_MSG = 0x01, UBX_ID_CFG_CFG = 0x09, UBX_ID_CFG_SBAS = 0x16, - UBX_ID_CFG_GNSS = 0x3E + UBX_ID_CFG_GNSS = 0x3E, + UBX_ID_CFG_PRT = 0x00 } ubx_class_cfg_id; typedef enum { @@ -129,6 +130,39 @@ typedef enum { UBX_ID_RXM_SFRB = 0x11, UBX_ID_RXM_SVSI = 0x20, } ubx_class_rxm_id; + +typedef enum { + UBX_GNSS_ID_GPS = 0, + UBX_GNSS_ID_SBAS = 1, + UBX_GNSS_ID_GALILEO = 2, + UBX_GNSS_ID_BEIDOU = 3, + UBX_GNSS_ID_IMES = 4, + UBX_GNSS_ID_QZSS = 5, + UBX_GNSS_ID_GLONASS = 6, + UBX_GNSS_ID_MAX +} ubx_config_gnss_id_t; + +// Enumeration options for field UBXDynamicModel +typedef enum { + UBX_DYNMODEL_PORTABLE = 0, + UBX_DYNMODEL_STATIONARY = 2, + UBX_DYNMODEL_PEDESTRIAN = 3, + UBX_DYNMODEL_AUTOMOTIVE = 4, + UBX_DYNMODEL_SEA = 5, + UBX_DYNMODEL_AIRBORNE1G = 6, + UBX_DYNMODEL_AIRBORNE2G = 7, + UBX_DYNMODEL_AIRBORNE4G = 8 +} ubx_config_dynamicmodel_t; + +typedef enum { + UBX_SBAS_SATS_AUTOSCAN = 0, + UBX_SBAS_SATS_WAAS = 1, + UBX_SBAS_SATS_EGNOS = 2, + UBX_SBAS_SATS_MSAS = 3, + UBX_SBAS_SATS_GAGAN = 4, + UBX_SBAS_SATS_SDCM = 5 +} ubx_config_sats_t; + // private structures // Geodetic Position Solution @@ -140,10 +174,9 @@ struct UBX_NAV_POSLLH { int32_t hMSL; // Height above mean sea level (mm) uint32_t hAcc; // Horizontal Accuracy Estimate (mm) uint32_t vAcc; // Vertical Accuracy Estimate (mm) -}; +} __attribute__((packed)); // Receiver Navigation Status - #define STATUS_GPSFIX_NOFIX 0x00 #define STATUS_GPSFIX_DRONLY 0x01 #define STATUS_GPSFIX_2DFIX 0x02 @@ -164,7 +197,7 @@ struct UBX_NAV_STATUS { uint8_t flags2; // Additional navigation output information uint32_t ttff; // Time to first fix (ms) uint32_t msss; // Milliseconds since startup/reset (ms) -}; +} __attribute__((packed)); // Dilution of precision struct UBX_NAV_DOP { @@ -176,10 +209,9 @@ struct UBX_NAV_DOP { uint16_t hDOP; // Horizontal DOP uint16_t nDOP; // Northing DOP uint16_t eDOP; // Easting DOP -}; +} __attribute__((packed)); // Navigation solution - struct UBX_NAV_SOL { uint32_t iTOW; // GPS Millisecond Time of Week (ms) int32_t fTOW; // fractional nanoseconds (ns) @@ -198,41 +230,9 @@ struct UBX_NAV_SOL { uint8_t reserved1; // Reserved uint8_t numSV; // Number of SVs used in Nav Solution uint32_t reserved2; // Reserved -}; - -// North/East/Down velocity - -struct UBX_NAV_VELNED { - uint32_t iTOW; // ms GPS Millisecond Time of Week - int32_t velN; // cm/s NED north velocity - int32_t velE; // cm/s NED east velocity - int32_t velD; // cm/s NED down velocity - uint32_t speed; // cm/s Speed (3-D) - uint32_t gSpeed; // cm/s Ground Speed (2-D) - int32_t heading; // 1e-5 *deg Heading of motion 2-D - uint32_t sAcc; // cm/s Speed Accuracy Estimate - uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate -}; - -// UTC Time Solution - -#define TIMEUTC_VALIDTOW (1 << 0) -#define TIMEUTC_VALIDWKN (1 << 1) -#define TIMEUTC_VALIDUTC (1 << 2) - -struct UBX_NAV_TIMEUTC { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint32_t tAcc; // Time Accuracy Estimate (ns) - int32_t nano; // Nanoseconds of second - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; // Validity Flags -}; +} __attribute__((packed)); +// PVT Navigation Position Velocity Time Solution #define PVT_VALID_VALIDDATE 0x01 #define PVT_VALID_VALIDTIME 0x02 #define PVT_VALID_FULLYRESOLVED 0x04 @@ -252,7 +252,6 @@ struct UBX_NAV_TIMEUTC { #define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2) #define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2) -// PVT Navigation Position Velocity Time Solution struct UBX_NAV_PVT { uint32_t iTOW; uint16_t year; @@ -286,10 +285,39 @@ struct UBX_NAV_PVT { uint32_t reserved3; } __attribute__((packed)); +// North/East/Down velocity +struct UBX_NAV_VELNED { + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate +} __attribute__((packed)); + +// UTC Time Solution +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) + +struct UBX_NAV_TIMEUTC { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags +} __attribute__((packed)); + // Space Vehicle (SV) Information - // Single SV information block - #define SVUSED (1 << 0) // This SV is used for navigation #define DIFFCORR (1 << 1) // Differential correction available #define ORBITAVAIL (1 << 2) // Orbit information available @@ -308,7 +336,7 @@ struct UBX_NAV_SVINFO_SV { int8_t elev; // Elevation (integer degrees) int16_t azim; // Azimuth (integer degrees) int32_t prRes; // Pseudo range residual (cm) -}; +} __attribute__((packed)); // SV information message #define MAX_SVS 32 @@ -319,19 +347,180 @@ struct UBX_NAV_SVINFO { uint8_t globalFlags; // uint16_t reserved2; // Reserved struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times -}; +} __attribute__((packed)); // ACK message class - struct UBX_ACK_ACK { uint8_t clsID; // ClassID uint8_t msgID; // MessageID -}; +} __attribute__((packed)); struct UBX_ACK_NAK { uint8_t clsID; // ClassID uint8_t msgID; // MessageID -}; +} __attribute__((packed)); + +// Communication port information +// default cfg_prt packet at 9600,N,8,1 (from u-center) +// 0000 B5 62 06 00 14 00-01 00 +// 0008 00 00 D0 08 00 00 80 25 +// 0010 00 00 07 00 03 00 00 00 +// 0018 00 00-A2 B5 +#define UBX_CFG_PRT_PORTID_DDC 0 +#define UBX_CFG_PRT_PORTID_UART1 1 +#define UBX_CFG_PRT_PORTID_UART2 2 +#define UBX_CFG_PRT_PORTID_USB 3 +#define UBX_CFG_PRT_PORTID_SPI 4 +#define UBX_CFG_PRT_MODE_DATABITS5 0x00 +#define UBX_CFG_PRT_MODE_DATABITS6 0x40 +#define UBX_CFG_PRT_MODE_DATABITS7 0x80 +#define UBX_CFG_PRT_MODE_DATABITS8 0xC0 +#define UBX_CFG_PRT_MODE_EVENPARITY 0x000 +#define UBX_CFG_PRT_MODE_ODDPARITY 0x200 +#define UBX_CFG_PRT_MODE_NOPARITY 0x800 +#define UBX_CFG_PRT_MODE_STOPBITS1_0 0x0000 +#define UBX_CFG_PRT_MODE_STOPBITS1_5 0x1000 +#define UBX_CFG_PRT_MODE_STOPBITS2_0 0x2000 +#define UBX_CFG_PRT_MODE_STOPBITS0_5 0x3000 +#define UBX_CFG_PRT_MODE_RESERVED 0x10 + +#define UBX_CFG_PRT_MODE_DEFAULT (UBX_CFG_PRT_MODE_DATABITS8 | UBX_CFG_PRT_MODE_NOPARITY | UBX_CFG_PRT_MODE_STOPBITS1_0 | UBX_CFG_PRT_MODE_RESERVED) + +struct UBX_CFG_PRT { + uint8_t portID; // 1 or 2 for UART ports + uint8_t res0; // reserved + uint16_t res1; // reserved + uint32_t mode; // bit masks for databits, stopbits, parity, and non-zero reserved + uint32_t baudRate; // bits per second, 9600 means 9600 + uint16_t inProtoMask; // bit 0 on = UBX, bit 1 on = NEMA + uint16_t outProtoMask; // bit 0 on = UBX, bit 1 on = NEMA + uint16_t flags; // reserved + uint16_t pad; // reserved +} __attribute__((packed)); + +struct UBX_CFG_MSG { + uint8_t msgClass; + uint8_t msgID; + uint8_t rate; +} __attribute__((packed)); + +struct UBX_CFG_RATE { + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; +} __attribute__((packed)); + +// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash +#define UBX_CFG_CFG_DEVICE_BBR 0x01 +#define UBX_CFG_CFG_DEVICE_FLASH 0x02 +#define UBX_CFG_CFG_DEVICE_EEPROM 0x04 +#define UBX_CFG_CFG_DEVICE_SPIFLASH 0x10 +#define UBX_CFG_CFG_DEVICE_ALL (UBX_CFG_CFG_DEVICE_BBR | UBX_CFG_CFG_DEVICE_FLASH | UBX_CFG_CFG_DEVICE_EEPROM | UBX_CFG_CFG_DEVICE_SPIFLASH) +#define UBX_CFG_CFG_SETTINGS_NONE 0x000 +#define UBX_CFG_CFG_SETTINGS_IOPORT 0x001 +#define UBX_CFG_CFG_SETTINGS_MSGCONF 0x002 +#define UBX_CFG_CFG_SETTINGS_INFMSG 0x004 +#define UBX_CFG_CFG_SETTINGS_NAVCONF 0x008 +#define UBX_CFG_CFG_SETTINGS_TPCONF 0x010 +#define UBX_CFG_CFG_SETTINGS_SFDRCONF 0x100 +#define UBX_CFG_CFG_SETTINGS_RINVCONF 0x200 +#define UBX_CFG_CFG_SETTINGS_ANTCONF 0x400 + +#define UBX_CFG_CFG_SETTINGS_ALL \ + (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_INFMSG | \ + UBX_CFG_CFG_SETTINGS_NAVCONF | \ + UBX_CFG_CFG_SETTINGS_TPCONF | \ + UBX_CFG_CFG_SETTINGS_SFDRCONF | \ + UBX_CFG_CFG_SETTINGS_RINVCONF | \ + UBX_CFG_CFG_SETTINGS_ANTCONF) + +// Sent messages for configuration support +struct UBX_CFG_CFG { + uint32_t clearMask; + uint32_t saveMask; + uint32_t loadMask; + uint8_t deviceMask; +} __attribute__((packed)); + +#define UBX_CFG_SBAS_MODE_ENABLED 0x01 +#define UBX_CFG_SBAS_MODE_TEST 0x02 +#define UBX_CFG_SBAS_USAGE_RANGE 0x01 +#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02 +#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04 + +// SBAS used satellite PNR bitmask (120-151) +// -------------------------------------1---------1---------1---------1 +// -------------------------------------5---------4---------3---------2 +// ------------------------------------10987654321098765432109876543210 +// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100 +// EGNOS 120, 124, 126, 131-------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001 +// MSAS 129, 137------------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000 +// GAGAN 127, 128-----------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000 +// SDCM 125, 140, 141-------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000 + +#define UBX_CFG_SBAS_SCANMODE2 0x00 +struct UBX_CFG_SBAS { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} __attribute__((packed)); + +#define UBX_CFG_GNSS_FLAGS_ENABLED 0x000001 +#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I 0x010000 +#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000 +#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000 + +#define UBX_CFG_GNSS_NUMCH_VER7 22 +#define UBX_CFG_GNSS_NUMCH_VER8 32 + +struct UBX_CFG_GNSS_CFGBLOCK { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t resvd; + uint32_t flags; +} __attribute__((packed)); + +struct UBX_CFG_GNSS { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + struct UBX_CFG_GNSS_CFGBLOCK cfgBlocks[UBX_GNSS_ID_MAX]; +} __attribute__((packed)); + +struct UBX_CFG_NAV5 { + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint8_t cnoThreshNumSVs; + uint8_t cnoThresh; + uint16_t reserved2; + uint32_t reserved3; + uint32_t reserved4; +} __attribute__((packed)); // MON message Class #define UBX_MON_MAX_EXT 5 @@ -341,8 +530,7 @@ struct UBX_MON_VER { #if UBX_MON_MAX_EXT > 0 char extension[UBX_MON_MAX_EXT][30]; #endif -}; - +} __attribute__((packed)); // OP custom messages struct UBX_OP_SYSINFO { @@ -360,7 +548,7 @@ struct UBX_OP_MAG { int16_t y; int16_t z; uint16_t Status; -}; +} __attribute__((packed)); typedef union { uint8_t payload[0]; @@ -374,6 +562,7 @@ typedef union { struct UBX_NAV_PVT nav_pvt; struct UBX_NAV_TIMEUTC nav_timeutc; struct UBX_NAV_SVINFO nav_svinfo; + struct UBX_CFG_PRT cfg_prt; // Ack Class struct UBX_ACK_ACK ack_ack; struct UBX_ACK_NAK ack_nak; @@ -390,15 +579,40 @@ struct UBXHeader { uint16_t len; uint8_t ck_a; uint8_t ck_b; -}; +} __attribute__((packed)); struct UBXPacket { struct UBXHeader header; UBXPayload payload; -}; +} __attribute__((packed)); + +struct UBXSENTHEADER { + uint8_t prolog[2]; + uint8_t class; + uint8_t id; + uint16_t len; +} __attribute__((packed)); + +union UBXSENTPACKET { + uint8_t buffer[0]; + struct { + struct UBXSENTHEADER header; + union { + struct UBX_CFG_CFG cfg_cfg; + struct UBX_CFG_MSG cfg_msg; + struct UBX_CFG_NAV5 cfg_nav5; + struct UBX_CFG_PRT cfg_prt; + struct UBX_CFG_RATE cfg_rate; + struct UBX_CFG_SBAS cfg_sbas; + struct UBX_CFG_GNSS cfg_gnss; + } payload; + uint8_t resvd[2]; // added space for checksum bytes + } message; +} __attribute__((packed)); // Used by AutoConfig code extern int32_t ubxHwVersion; +extern GPSPositionSensorSensorTypeOptions sensorType; extern struct UBX_ACK_ACK ubxLastAck; extern struct UBX_ACK_NAK ubxLastNak; diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 0dcb13397..eb3412385 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -33,20 +33,48 @@ // defines // TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that. -#define UBX_MAX_RATE_VER8 18 -#define UBX_MAX_RATE_VER7 10 -#define UBX_MAX_RATE 5 +#define UBX_MAX_RATE_VER8 18 +#define UBX_MAX_RATE_VER7 10 +#define UBX_MAX_RATE 5 + +// reset to factory (and 9600 baud), and baud changes are not acked +// it could be 31 (full PIOS buffer) + 60 (gnss) + overhead bytes at 240 bytes per second +// = .42 seconds for the longest sentence to be fully transmitted +// and then it may have to do something like erase flash before replying... + +// at low baud rates and high data rates the ubx gps simply must drop some outgoing data +// and when a lot of data is being dropped, the MON VER reply often gets dropped +// on the other hand, uBlox documents that some versions discard data that is over a second old +// implying that it could be over 1 second before a reply is received +// later versions dropped this and drop data when the send buffer is full and that could be even longer +// rather than have long timeouts, we will let timeouts * retries handle that if it happens -// time to wait before reinitializing the fsm due to disconnection -#define UBX_CONNECTION_TIMEOUT (2000 * 1000) -// times between retries in case an error does occurs -#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000) // timeout for ack reception -#define UBX_REPLY_TIMEOUT (500 * 1000) +#define UBX_REPLY_TIMEOUT (500 * 1000) +// timeout for a settings save, in case it has to erase flash +#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000) // max retries in case of timeout -#define UBX_MAX_RETRIES 5 -// pause between each configuration step -#define UBX_STEP_WAIT_TIME (10 * 1000) +#define UBX_MAX_RETRIES 5 +// pause between each verifiably correct configuration step +#define UBX_VERIFIED_STEP_WAIT_TIME (50 * 1000) +// pause between each unverifiably correct configuration step +#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000) + +#define UBX_CFG_CFG_OP_STORE_SETTINGS \ + (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_NAVCONF) +#define UBX_CFG_CFG_OP_CLEAR_SETTINGS ((~UBX_CFG_CFG_OP_STORE_SETTINGS) & UBX_CFG_CFG_SETTINGS_ALL) +// don't clear rinv as that is just text as configured by the owner +#define UBX_CFG_CFG_OP_RESET_SETTINGS \ + (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_INFMSG | \ + UBX_CFG_CFG_SETTINGS_NAVCONF | \ + UBX_CFG_CFG_SETTINGS_TPCONF | \ + UBX_CFG_CFG_SETTINGS_SFDRCONF | \ + UBX_CFG_CFG_SETTINGS_ANTCONF) + // types typedef enum { UBX_AUTOCONFIG_STATUS_DISABLED = 0, @@ -54,26 +82,6 @@ typedef enum { UBX_AUTOCONFIG_STATUS_DONE, UBX_AUTOCONFIG_STATUS_ERROR } ubx_autoconfig_status_t; -// Enumeration options for field UBXDynamicModel -typedef enum { - UBX_DYNMODEL_PORTABLE = 0, - UBX_DYNMODEL_STATIONARY = 2, - UBX_DYNMODEL_PEDESTRIAN = 3, - UBX_DYNMODEL_AUTOMOTIVE = 4, - UBX_DYNMODEL_SEA = 5, - UBX_DYNMODEL_AIRBORNE1G = 6, - UBX_DYNMODEL_AIRBORNE2G = 7, - UBX_DYNMODEL_AIRBORNE4G = 8 -} ubx_config_dynamicmodel_t; - -typedef enum { - UBX_SBAS_SATS_AUTOSCAN = 0, - UBX_SBAS_SATS_WAAS = 1, - UBX_SBAS_SATS_EGNOS = 2, - UBX_SBAS_SATS_MSAS = 3, - UBX_SBAS_SATS_GAGAN = 4, - UBX_SBAS_SATS_SDCM = 5 -} ubx_config_sats_t; #define UBX_ typedef struct { @@ -94,142 +102,21 @@ typedef struct { bool enableBeiDou; } ubx_autoconfig_settings_t; -// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash -#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10) - // Sent messages for configuration support -typedef struct { - uint32_t clearMask; - uint32_t saveMask; - uint32_t loadMask; - uint8_t deviceMask; -} __attribute__((packed)) ubx_cfg_cfg_t; +typedef struct UBX_CFG_CFG ubx_cfg_cfg_t; +typedef struct UBX_CFG_NAV5 ubx_cfg_nav5_t; +typedef struct UBX_CFG_RATE ubx_cfg_rate_t; +typedef struct UBX_CFG_MSG ubx_cfg_msg_t; +typedef struct UBX_CFG_PRT ubx_cfg_prt_t; +typedef struct UBX_CFG_SBAS ubx_cfg_sbas_t; +typedef struct UBX_CFG_GNSS_CFGBLOCK ubx_cfg_gnss_cfgblock_t; +typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t; +typedef struct UBXSENTHEADER UBXSentHeader_t; +typedef union UBXSENTPACKET UBXSentPacket_t; -typedef struct { - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint8_t cnoThreshNumSVs; - uint8_t cnoThresh; - uint16_t reserved2; - uint32_t reserved3; - uint32_t reserved4; -} __attribute__((packed)) ubx_cfg_nav5_t; +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); +void ubx_autoconfig_set(ubx_autoconfig_settings_t *config); +void ubx_reset_sensor_type(); -typedef struct { - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; -} __attribute__((packed)) ubx_cfg_rate_t; - -typedef struct { - uint8_t msgClass; - uint8_t msgID; - uint8_t rate; -} __attribute__((packed)) ubx_cfg_msg_t; - -#define UBX_CFG_SBAS_MODE_ENABLED 0x01 -#define UBX_CFG_SBAS_MODE_TEST 0x02 -#define UBX_CFG_SBAS_USAGE_RANGE 0x01 -#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02 -#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04 - -// SBAS used satellite PNR bitmask (120-151) -// -------------------------------------1---------1---------1---------1 -// -------------------------------------5---------4---------3---------2 -// ------------------------------------10987654321098765432109876543210 -// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100 -// EGNOS 120, 124, 126, 131-------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001 -// MSAS 129, 137------------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000 -// GAGAN 127, 128-----------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000 -// SDCM 125, 140, 141-------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000 - -#define UBX_CFG_SBAS_SCANMODE2 0x00 -typedef struct { - uint8_t mode; - uint8_t usage; - uint8_t maxSBAS; - uint8_t scanmode2; - uint32_t scanmode1; -} __attribute__((packed)) ubx_cfg_sbas_t; - -typedef enum { - UBX_GNSS_ID_GPS = 0, - UBX_GNSS_ID_SBAS = 1, - UBX_GNSS_ID_GALILEO = 2, - UBX_GNSS_ID_BEIDOU = 3, - UBX_GNSS_ID_IMES = 4, - UBX_GNSS_ID_QZSS = 5, - UBX_GNSS_ID_GLONASS = 6, - UBX_GNSS_ID_MAX -} ubx_config_gnss_id_t; - -#define UBX_CFG_GNSS_FLAGS_ENABLED 0x01 -#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I 0x010000 -#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000 -#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000 - -#define UBX_CFG_GNSS_NUMCH_VER7 22 -#define UBX_CFG_GNSS_NUMCH_VER8 32 - -typedef struct { - uint8_t gnssId; - uint8_t resTrkCh; - uint8_t maxTrkCh; - uint8_t resvd; - uint32_t flags; -} __attribute__((packed)) ubx_cfg_gnss_cfgblock_t; - -typedef struct { - uint8_t msgVer; - uint8_t numTrkChHw; - uint8_t numTrkChUse; - uint8_t numConfigBlocks; - ubx_cfg_gnss_cfgblock_t cfgBlocks[UBX_GNSS_ID_MAX]; -} __attribute__((packed)) ubx_cfg_gnss_t; - -typedef struct { - uint8_t prolog[2]; - uint8_t class; - uint8_t id; - uint16_t len; -} __attribute__((packed)) UBXSentHeader_t; - -typedef union { - uint8_t buffer[0]; - struct { - UBXSentHeader_t header; - union { - ubx_cfg_cfg_t cfg_cfg; - ubx_cfg_msg_t cfg_msg; - ubx_cfg_nav5_t cfg_nav5; - ubx_cfg_rate_t cfg_rate; - ubx_cfg_sbas_t cfg_sbas; - ubx_cfg_gnss_t cfg_gnss; - } payload; - uint8_t resvd[2]; // added space for checksum bytes - } message; -} __attribute__((packed)) UBXSentPacket_t; - -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected); -void ubx_autoconfig_set(ubx_autoconfig_settings_t config); int32_t ubx_autoconfig_get_status(); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index c86fbb445..a38c7b52b 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -27,29 +27,48 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include +#include "hwsettings.h" + #include "inc/ubx_autoconfig.h" #include + // private type definitions + typedef enum { INIT_STEP_DISABLED = 0, INIT_STEP_START, - INIT_STEP_ASK_VER, - INIT_STEP_WAIT_VER, + INIT_STEP_RESET_GPS, + INIT_STEP_REVO_9600_BAUD, + INIT_STEP_GPS_BAUD, + INIT_STEP_REVO_BAUD, INIT_STEP_ENABLE_SENTENCES, INIT_STEP_ENABLE_SENTENCES_WAIT_ACK, INIT_STEP_CONFIGURE, INIT_STEP_CONFIGURE_WAIT_ACK, + INIT_STEP_SAVE, + INIT_STEP_SAVE_WAIT_ACK, INIT_STEP_DONE, - INIT_STEP_ERROR, + INIT_STEP_ERROR } initSteps_t; typedef struct { - initSteps_t currentStep; // Current configuration "fsm" status - uint32_t lastStepTimestampRaw; // timestamp of last operation - uint32_t lastConnectedRaw; // timestamp of last time gps was connected - UBXSentPacket_t working_packet; // outbound "buffer" - ubx_autoconfig_settings_t currentSettings; - int8_t lastConfigSent; // index of last configuration string sent + initSteps_t currentStep; // Current configuration "fsm" status + initSteps_t currentStepSave; // Current configuration "fsm" status + uint32_t lastStepTimestampRaw; // timestamp of last operation + uint32_t lastConnectedRaw; // timestamp of last time gps was connected + struct { + UBXSentPacket_t working_packet; // outbound "buffer" + // bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800 + // failure mode otherwise: + // - send message with baud rate change + // - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly) + // - change Revo port baud rate + // sometimes fails (much worse for lowest baud rates) + uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+ + } __attribute__((packed)); + volatile ubx_autoconfig_settings_t currentSettings; + int8_t lastConfigSent; // index of last configuration string sent struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS uint8_t retryCount; } status_t; @@ -114,15 +133,24 @@ ubx_cfg_msg_t msg_config_ubx7[] = { }; // private defines + #define LAST_CONFIG_SENT_START (-1) #define LAST_CONFIG_SENT_COMPLETED (-2) +// always reset the stored GPS configuration, even when doing autoconfig.nostore +// that is required to do a 100% correct configuration +// but is unexpected because it changes the stored configuration when doing autoconfig.nostore +// note that a reset is always done with autoconfig.store +// #define ALWAYS_RESET // private variables // enable the autoconfiguration system -static bool enabled; +static volatile bool enabled = false; +static volatile bool current_step_touched = false; +// both the pointer and what it points to are volatile. Yuk. +static volatile status_t *volatile status = 0; +static uint8_t hwsettings_baud; -static status_t *status = 0; static void append_checksum(UBXSentPacket_t *packet) { @@ -139,29 +167,152 @@ static void append_checksum(UBXSentPacket_t *packet) packet->buffer[len] = ck_a; packet->buffer[len + 1] = ck_b; } + + /** * prepare a packet to be sent, fill the header and appends the checksum. * return the total packet lenght comprising header and checksum */ static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len) { + memset((uint8_t *)status->working_packet.buffer + len + sizeof(UBXSentHeader_t) + 2, 0, sizeof(status->bufferPaddingForPiosBugAt2400Baud)); packet->message.header.prolog[0] = UBX_SYNC1; packet->message.header.prolog[1] = UBX_SYNC2; packet->message.header.class = classID; packet->message.header.id = messageID; packet->message.header.len = len; append_checksum(packet); - return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum + + status->requiredAck.clsID = classID; + status->requiredAck.msgID = messageID; + + return len + sizeof(UBXSentHeader_t) + 2 + sizeof(status->bufferPaddingForPiosBugAt2400Baud); // payload + header + checksum + extra bytes } + static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send) { *bytes_to_send = prepare_packet(packet, classID, messageID, 0); } -void config_rate(uint16_t *bytes_to_send) + +static void set_current_step_if_untouched(initSteps_t new_steps) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); + // assume this one byte initSteps_t is atomic + // take care of some but not all concurrency issues + + if (!current_step_touched) { + status->currentStep = new_steps; + } + if (current_step_touched) { + status->currentStep = status->currentStepSave; + } +} + + +void ubx_reset_sensor_type() +{ + ubxHwVersion = -1; + sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; + GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); +} + + +static void config_reset(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); + // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB + // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400 + // first: reset (permanent settings to default) all but rinv = e.g. owner name + status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_RESET_SETTINGS; + // then: don't store any current settings to permanent + status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_SETTINGS_NONE; + // lastly: load (immediately start to use) all but rinv = e.g. owner name + status->working_packet.message.payload.cfg_cfg.loadMask = UBX_CFG_CFG_OP_RESET_SETTINGS; + // all devices + status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); +} + + +// set the GPS baud rate to the user specified baud rate +// because we may have started up with 9600 baud (for a GPS with no permanent settings) +static void config_gps_baud(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t)); + status->working_packet.message.payload.cfg_prt.mode = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved + status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2 + status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0) + status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0) + // Ask GPS to change it's speed + switch (hwsettings_baud) { + case HWSETTINGS_GPSSPEED_2400: + status->working_packet.message.payload.cfg_prt.baudRate = 2400; + break; + case HWSETTINGS_GPSSPEED_4800: + status->working_packet.message.payload.cfg_prt.baudRate = 4800; + break; + case HWSETTINGS_GPSSPEED_9600: + status->working_packet.message.payload.cfg_prt.baudRate = 9600; + break; + case HWSETTINGS_GPSSPEED_19200: + status->working_packet.message.payload.cfg_prt.baudRate = 19200; + break; + case HWSETTINGS_GPSSPEED_38400: + status->working_packet.message.payload.cfg_prt.baudRate = 38400; + break; + case HWSETTINGS_GPSSPEED_57600: + status->working_packet.message.payload.cfg_prt.baudRate = 57600; + break; + case HWSETTINGS_GPSSPEED_115200: + status->working_packet.message.payload.cfg_prt.baudRate = 115200; + break; + case HWSETTINGS_GPSSPEED_230400: + status->working_packet.message.payload.cfg_prt.baudRate = 230400; + break; + } + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t)); +} + + +// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate +static void config_baud(uint8_t baud) +{ + // Set Revo port hwsettings_baud + switch (baud) { + case HWSETTINGS_GPSSPEED_2400: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400); + break; + case HWSETTINGS_GPSSPEED_4800: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800); + break; + case HWSETTINGS_GPSSPEED_9600: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600); + break; + case HWSETTINGS_GPSSPEED_19200: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200); + break; + case HWSETTINGS_GPSSPEED_38400: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400); + break; + case HWSETTINGS_GPSSPEED_57600: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600); + break; + case HWSETTINGS_GPSSPEED_115200: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200); + break; + case HWSETTINGS_GPSSPEED_230400: + PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400); + break; + } +} + + +static void config_rate(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); // if rate is less than 1 uses the highest rate for current hardware uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99; if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) { @@ -172,36 +323,31 @@ void config_rate(uint16_t *bytes_to_send) rate = UBX_MAX_RATE_VER8; } uint16_t period = 1000 / rate; - status->working_packet.message.payload.cfg_rate.measRate = period; status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1 status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_RATE; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); } -void config_nav(uint16_t *bytes_to_send) -{ - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t)); +static void config_nav(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t)); status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel; status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB - status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_NAV5; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); } -void config_sbas(uint16_t *bytes_to_send) + +static void config_sbas(uint16_t *bytes_to_send) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t)); - - status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ? - status->currentSettings.SBASChannelsUsed : 3; - + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t)); + status->working_packet.message.payload.cfg_sbas.maxSBAS = + status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3; status->working_packet.message.payload.cfg_sbas.usage = (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) | (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) | @@ -209,34 +355,28 @@ void config_sbas(uint16_t *bytes_to_send) // If sbas is used for anything then set mode as enabled status->working_packet.message.payload.cfg_sbas.mode = status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0; - status->working_packet.message.payload.cfg_sbas.scanmode1 = status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS : status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS : status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS : status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN : status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN; + status->working_packet.message.payload.cfg_sbas.scanmode2 = + UBX_CFG_SBAS_SCANMODE2; - status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2; - - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_SBAS; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); } -void config_gnss(uint16_t *bytes_to_send) + +static void config_gnss(uint16_t *bytes_to_send) { - int32_t i; - - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t)); - + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t)); status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX; status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7; status->working_packet.message.payload.cfg_gnss.numTrkChUse = status->working_packet.message.payload.cfg_gnss.numTrkChHw; - for (i = 0; i < UBX_GNSS_ID_MAX; i++) { + for (int32_t i = 0; i < UBX_GNSS_ID_MAX; i++) { status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i; - switch (i) { case UBX_GNSS_ID_GPS: if (status->currentSettings.enableGPS) { @@ -278,30 +418,35 @@ void config_gnss(uint16_t *bytes_to_send) } } - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_GNSS; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); } -void config_save(uint16_t *bytes_to_send) + +static void config_save(uint16_t *bytes_to_send) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB - status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf - status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK; - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_CFG; + // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400 + status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_OP_STORE_SETTINGS; // a list of settings we just set + status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_CLEAR_SETTINGS; // everything else gets factory default + status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); } + + static void configure(uint16_t *bytes_to_send) { switch (status->lastConfigSent) { case LAST_CONFIG_SENT_START: + // increase message rates to 5 fixes per second config_rate(bytes_to_send); break; + case LAST_CONFIG_SENT_START + 1: config_nav(bytes_to_send); break; + case LAST_CONFIG_SENT_START + 2: if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) { config_gnss(bytes_to_send); @@ -310,24 +455,19 @@ static void configure(uint16_t *bytes_to_send) // Skip and fall through to next step status->lastConfigSent++; } + // in the else case we must fall through because we must send something each time because successful send is tested externally + case LAST_CONFIG_SENT_START + 3: config_sbas(bytes_to_send); - if (!status->currentSettings.storeSettings) { - // skips saving - status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - } break; - case LAST_CONFIG_SENT_START + 4: - config_save(bytes_to_send); - status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - return; default: status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - return; + break; } } + static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) { int8_t msg = status->lastConfigSent + 1; @@ -338,66 +478,303 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) if (msg >= 0 && msg < msg_count) { status->working_packet.message.payload.cfg_msg = msg_config[msg]; - - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_MSG; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); } else { status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } } -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected) + +// End User Documentation + +// There are two baud rates of interest +// The baud rate the GPS is talking at +// The baud rate Revo is talking at +// These two must match for the GPS to work +// You only have direct control of the Revo baud rate +// The two baud rates must be the same for the Revo to send a command to the GPS +// to tell the GPS to change it's baud rate +// So you start out by changing Revo's baud rate to match the GPS's +// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud +// That is the basis of these instructions + +// There are microprocessors and they each have internal settings +// Revo +// GPS +// and each of these settings can be temporary or permanent + +// To change a Revo setting +// Use the System tab in the GCS for all the following +// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled +// Click on UbxAutoConfig itself and the line will turn green and blue +// To change this setting permanently, press the red up arrow (Save) at the top of the screen +// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on +// To change this setting temporarily, press the green up arrow (Send) at the top of the screen +// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on + +// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes +// This only works correctly after you have matching baud rates so Revo and GPS can talk together +// "Settings->GPSSettings->UbxAutoConfig = Configure" sets the internal GPS setting temporarily +// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently + +// You want to wind up with a set of permanent settings that work together +// There are two different sets of permanent settings that work together +// GPS at 9600 baud and factory defaults +// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud +// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway) +// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings +// GPS at 57600 baud with all the settings for the current release stored in the GPS +// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly +// May require reconfiguring in a future release + +// Changable settings of interest +// AutoConfig mode +// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure) +// Disabled means that changes to the GPS baud setting only affect the Revo port +// It doesn't try to change the GPS's internal baud rate setting +// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on) +// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on) +// GPS baud rate +// Settings->HwSettings->GPSSpeed +// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate +// If the baud rates are not the same and an AutoConfig mode is enabled it will fail +// If AutoConfig mode is disabled this will only change the Revo baud rate + +// View only settings of interest +// Detected GPS type +// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8) +// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking +// Real time progress of the GPS detection process +// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR) + +// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting +// This is necessary for the GPS to work with Revo +// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate) +// Temporarily change the AutoConfig mode to Disabled +// Temporarily change the GPS baud rate to a value you think it might be (or go up the list) +// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown) +// Repeat, changing the GPS baud rate, until found + +// Some very important facts: +// For 9600 baud or lower, the autoconfig will configure it to factory default settings +// For 19200 baud or higher, the autoconfig will configure it to OP required settings +// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud +// 57600 baud is recommended for the current release +// That can be achieved either by +// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up +// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig +// Some previous releases used 38400 and had some other settings differences + +// The user should either: +// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by: +// (Recommended method because new versions could require new settings and this handles future changes automatically) +// Syncing the baud rates +// Setting it to autoconfig.nostore and waiting for it to complete +// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete +// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently) +// Permanently setting it to autoconfig.nostore and waiting for it to complete +// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete +// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by: +// (Less recommended method because new versions could require new settings so you would have to do this again) +// Syncing the baud rates +// Setting it to autoconfig.nostore and waiting for it to complete +// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete +// Setting it to autoconfig.store +// Permanently setting it to autoconfig.disabled + +// The algorithm is: +// If autoconfig is enabled at all +// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed +// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage) +// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used +// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping +// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship +// If autoconfig is not enabled +// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes +// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will +// 1 Reset the permanent configuration back to factory default +// 2 Disable NEMA message settings +// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running +// 4 Save the current volatile settings to non-volatile storage +// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will +// 2 Disable NEMA message settings +// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running +// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings + +// Talking points to point out: +// U-center is no longer needed for any use case with this code +// 9600 is factory default for GPS's +// Some GPS can't even permanently store settings and must start at 9600 baud? +// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :) +// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings) +// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud) +// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings +// GPS's with 9600 baud and factory default settings would be a good default for future OP releases +// Changing the GPS internal settings multiple times in the future is handled automatically +// This code is written to do a configure from 9600 to 57600 +// (actually 9600 to whatever is stored in HwSettings.GPSSpeed) +// if autoconfig is enabled at boot up +// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings +// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default +// For 19200 baud or higher, the autoconfig will configure it to OP required settings +// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud +// This is good for factory default GPS's +// This is good in case we change some settings in a future release + + +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; *buffer = (char *)status->working_packet.buffer; - if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) { - return; // autoconfig not enabled - } - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) { + current_step_touched = false; + + // autoconfig struct not yet allocated + if (!status) { return; } - // when gps is disconnected it will replay the autoconfig sequence. - if (!gps_connected) { - if (status->currentStep == INIT_STEP_DONE) { - // if some operation is in progress and it is not running into issues it maybe that - // the disconnection is only due to wrong message rates, so reinit only when done. - // errors caused by disconnection are handled by error retry logic - if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) { - status->currentStep = INIT_STEP_START; - return; - } + // smallest delay between each step + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) { + return; + } + // get UBX version whether autoconfig is enabled or not + // this allows the user to try some baud rates and visibly see when it works + // ubxHwVersion is a global set externally by the caller of this function + if (ubxHwVersion <= 0) { + // at low baud rates and high data rates the ubx gps simply must drop some outgoing data + // this isn't really an error + // and when a lot of data is being dropped, the MON VER reply often gets dropped + // on the other hand, uBlox documents that some versions discard data that is over 1 second old + // implying a 1 second send buffer and that it could be over 1 second before a reply is received + // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full + // and that could be even longer than 1 second + // send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped + + // wait for the normal reply timeout before sending it over and over + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) { + return; } - } else { - // reset connection timer - status->lastConnectedRaw = PIOS_DELAY_GetRaw(); + build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); + // keep timeouts running properly, we (will have) just sent a packet that generates a reply + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + return; + } + if (!enabled) { + // keep resetting the timeouts here if we are not actually going to run the configure code + // not really necessary, but it keeps the timer from wrapping every 50 seconds + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + return; // autoconfig not enabled + } + + // replaying constantly could wear the settings memory out + // don't allow constant reconfiging when offline + // don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring + if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) { + return; } switch (status->currentStep) { - case INIT_STEP_ERROR: - case INIT_STEP_DISABLED: - case INIT_STEP_DONE: - return; - case INIT_STEP_START: - case INIT_STEP_ASK_VER: - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); - status->currentStep = INIT_STEP_WAIT_VER; + // we should look for the GPS version again + ubx_reset_sensor_type(); + // do not fall through to next state + // or it might try to get the sensor type when the baud rate is half changed + set_current_step_if_untouched(INIT_STEP_RESET_GPS); + // allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); break; - case INIT_STEP_WAIT_VER: - if (ubxHwVersion > 0) { - status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentStep = INIT_STEP_ENABLE_SENTENCES; + case INIT_STEP_RESET_GPS: + // make sure we don't change the baud rate too soon and garble the packet being sent + // even after pios says the buffer is empty, the serial port buffer still has data in it + // and changing the baud will screw it up + // when the GPS is configured to send a lot of data, but has a low baud rate + // it has way too many messages to send and has to drop most of them + + // Retrieve desired GPS baud rate once for use throughout this module + HwSettingsGPSSpeedGet(&hwsettings_baud); +#if !defined(ALWAYS_RESET) + // ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore + // but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore + // if user requests a low baud rate then we just reset and leave it set to NEMA + // because low baud and high OP data rate doesn't play nice + // if user requests that settings be saved, we will reset here too + // that makes sure that all strange settings are reset to factory default + // else these strange settings may persist because we don't reset all settings by table + if (status->currentSettings.storeSettings) +#endif + { + // reset all GPS parameters to factory default (configure low rate NEMA for low baud rates) + // this is not usable by OP code for either baud rate or types of messages sent + // but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages) + config_reset(bytes_to_send); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + // else allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD); + break; + + case INIT_STEP_REVO_9600_BAUD: +#if !defined(ALWAYS_RESET) + // if user requests a low baud rate then we just reset and leave it set to NEMA + // because low baud and high OP data rate doesn't play nice + // if user requests that settings be saved, we will reset here too + // that makes sure that all strange settings are reset to factory default + // else these strange settings may persist because we don't reset all settings by hand + if (status->currentSettings.storeSettings) +#endif + { + // wait for previous step + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { + return; + } + // set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done + config_baud(HWSETTINGS_GPSSPEED_9600); + } + // at most, we just set Revo baud and that doesn't send any data + // fall through to next state + // we can do that if we choose because we haven't sent any data in this state + // set_current_step_if_untouched(INIT_STEP_GPS_BAUD); + // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // break; + + case INIT_STEP_GPS_BAUD: + // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could + // affect baud rate and other transmission parameters. Because there may be messages queued for transmission + // there may be uncertainty about which protocol applies to such messages. In addition a message currently in + // transmission may be corrupted by a protocol change. Host data reception parameters may have to be changed to + // be able to receive future messages, including the acknowledge message associated with the UBX-CFG-CFG message. + + // so the message that changes the baud rate will send it's acknowledgement back at the new baud rate; this is not good. + // if your message was corrupted, you didn't change the baud rate and you have to guess; try pinging at both baud rates. + // also, you would have to change the baud rate instantly after the last byte of the sentence was sent, + // and you would have to poll the port in real time for that, and there may be messages ahead of the baud rate change. + // + // so we ignore the ack from this. it has proven to be reliable (with the addition of two dummy bytes after the packet) + + // set the GPS internal baud rate to the user configured value + config_gps_baud(bytes_to_send); + set_current_step_if_untouched(INIT_STEP_REVO_BAUD); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; + + case INIT_STEP_REVO_BAUD: + // wait for previous step + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { return; } - - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { - status->currentStep = INIT_STEP_ASK_VER; + // set the Revo GPS port baud rate to the (same) user configured value + config_baud(hwsettings_baud); + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->retryCount = 0; + // skip enabling UBX sentences for low baud rates + // low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration + if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) { + set_current_step_if_untouched(INIT_STEP_SAVE); + } else { + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } - return; + // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; case INIT_STEP_ENABLE_SENTENCES: case INIT_STEP_CONFIGURE: @@ -409,61 +786,136 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec enable_sentences(bytes_to_send); } - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { - status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentStep = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE; + if (step_configure) { + // zero retries for the next state that needs it (INIT_STEP_SAVE) + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_SAVE); + } else { + // finished enabling sentences, now configure() needs to start at the beginning + status->lastConfigSent = LAST_CONFIG_SENT_START; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); + } } else { - status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK; + set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); } - return; + break; } + case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK: case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS { bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) { // Continue with next configuration option + // start retries over for the next setting to be sent status->retryCount = 0; status->lastConfigSent++; - } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT || - (ubxLastNak.clsID == status->requiredAck.clsID && ubxLastNak.msgID == status->requiredAck.msgID)) { + } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT && + (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) { + // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; + } else { // timeout or NAK, resend the message or abort status->retryCount++; if (status->retryCount > UBX_MAX_RETRIES) { - status->currentStep = INIT_STEP_ERROR; + set_current_step_if_untouched(INIT_STEP_ERROR); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - return; + break; } } - // no abort condition, continue or retries., + // success or failure here, retries are handled elsewhere if (step_configure) { - status->currentStep = INIT_STEP_CONFIGURE; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); } else { - status->currentStep = INIT_STEP_ENABLE_SENTENCES; + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } - return; + break; } + + case INIT_STEP_SAVE: + if (status->currentSettings.storeSettings) { + config_save(bytes_to_send); + set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } else { + set_current_step_if_untouched(INIT_STEP_DONE); + // allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + break; + + // we could remove this state + // if we retry, it writes to settings storage a few more times + // and it is probably the ack that was dropped, with the save actually performed correctly + case INIT_STEP_SAVE_WAIT_ACK: + if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) { + // Continue with next configuration option + set_current_step_if_untouched(INIT_STEP_DONE); + // note that we increase the reply timeout in case the GPS must do a flash erase + } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT && + (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) { + // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; + } else { + // timeout or NAK, resend the message or abort + status->retryCount++; + if (status->retryCount > UBX_MAX_RETRIES / 2) { + // give up on the retries + set_current_step_if_untouched(INIT_STEP_ERROR); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } else { + // retry a few times + set_current_step_if_untouched(INIT_STEP_SAVE); + } + } + break; + + case INIT_STEP_ERROR: + // on error we should get the GPS version immediately + ubx_reset_sensor_type(); + // fall through + case INIT_STEP_DISABLED: + case INIT_STEP_DONE: + break; } } -void ubx_autoconfig_set(ubx_autoconfig_settings_t config) + +void ubx_autoconfig_set(ubx_autoconfig_settings_t *config) { + initSteps_t new_step; + enabled = false; - if (config.autoconfigEnabled) { - if (!status) { - status = (status_t *)pios_malloc(sizeof(status_t)); - PIOS_Assert(status); - memset(status, 0, sizeof(status_t)); - status->currentStep = INIT_STEP_DISABLED; - } - status->currentSettings = config; - status->currentStep = INIT_STEP_START; - enabled = true; + + if (!status) { + status = (status_t *)pios_malloc(sizeof(status_t)); + PIOS_Assert(status); + memset((status_t *)status, 0, sizeof(status_t)); + } + + // if caller used NULL, just use current settings to restart autoconfig process + if (config != NULL) { + status->currentSettings = *config; + } + if (status->currentSettings.autoconfigEnabled) { + new_step = INIT_STEP_START; } else { - if (status) { - status->currentStep = INIT_STEP_DISABLED; - } + new_step = INIT_STEP_DISABLED; + } + + // assume this one byte initSteps_t is atomic + // take care of some but not all concurrency issues + + status->currentStep = new_step; + status->currentStepSave = new_step; + current_step_touched = true; + status->currentStep = new_step; + status->currentStepSave = new_step; + + if (status->currentSettings.autoconfigEnabled) { + enabled = true; } } diff --git a/flight/modules/PathFollower/inc/pathfollowerfsm.h b/flight/modules/PathFollower/inc/pathfollowerfsm.h index 9701bc7de..0188cc71c 100644 --- a/flight/modules/PathFollower/inc/pathfollowerfsm.h +++ b/flight/modules/PathFollower/inc/pathfollowerfsm.h @@ -34,6 +34,7 @@ extern "C" { #include } +#include typedef enum { PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup @@ -42,7 +43,7 @@ typedef enum { PFFSM_STATE_ABORT // Abort on error } PathFollowerFSMState_T; -class PathFollowerFSM { +class PathFollowerFSM : public PIDControlDownCallback { public: // PathFollowerFSM() {}; virtual void Inactive(void) {} diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp index 729f65097..2c4bdc569 100644 --- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp @@ -126,7 +126,7 @@ void VtolAutoTakeoffController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp index 208e862f7..862011057 100644 --- a/flight/modules/PathFollower/vtolbrakecontroller.cpp +++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp @@ -132,7 +132,7 @@ void VtolBrakeController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd, - vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit, + vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta, dT, 10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp index e76c09d54..20a387277 100644 --- a/flight/modules/PathFollower/vtolflycontroller.cpp +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -127,7 +127,7 @@ void VtolFlyController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); @@ -136,7 +136,7 @@ void VtolFlyController::SettingsUpdated(void) controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp, vtolPathFollowerSettings->VerticalVelPID.Ki, vtolPathFollowerSettings->VerticalVelPID.Kd, - vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA + vtolPathFollowerSettings->VerticalVelPID.Beta, dT, vtolPathFollowerSettings->VerticalVelMax); controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); @@ -145,6 +145,9 @@ void VtolFlyController::SettingsUpdated(void) VtolSelfTuningStatsGet(&vtolSelfTuningStats); controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral); controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); + + // disable neutral thrust calcs which should only be done in a hold mode. + controlDown.DisableNeutralThrustCalc(); } /** diff --git a/flight/modules/PathFollower/vtollandcontroller.cpp b/flight/modules/PathFollower/vtollandcontroller.cpp index 96b4a40c7..97a7d466f 100644 --- a/flight/modules/PathFollower/vtollandcontroller.cpp +++ b/flight/modules/PathFollower/vtollandcontroller.cpp @@ -120,7 +120,7 @@ void VtolLandController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp index 84ba99daa..68018c5ea 100644 --- a/flight/modules/PathFollower/vtolvelocitycontroller.cpp +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -107,7 +107,7 @@ void VtolVelocityController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); @@ -181,7 +181,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; // default thrust mode to altvario diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index b0a440d43..9772d393e 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES @@ -102,12 +103,17 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; - uint8_t sample_count; + uint8_t sample_count; + uint8_t quality; }; static struct rcvr_activity_fsm activity_fsm; static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); +static void resetRcvrStatus(struct rcvr_activity_fsm *fsm); +static bool updateRcvrStatus( + struct rcvr_activity_fsm *fsm, + ManualControlSettingsChannelGroupsOptions group); #define assumptions \ ( \ @@ -143,6 +149,7 @@ int32_t ReceiverInitialize() AccessoryDesiredInitialize(); ManualControlCommandInitialize(); ReceiverActivityInitialize(); + ReceiverStatusInitialize(); ManualControlSettingsInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES StabilizationSettingsInitialize(); @@ -208,6 +215,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) /* Initialize the RcvrActivty FSM */ portTickType lastActivityTime = xTaskGetTickCount(); resetRcvrActivity(&activity_fsm); + resetRcvrStatus(&activity_fsm); // Main task loop lastSysTime = xTaskGetTickCount(); @@ -232,9 +240,13 @@ static void receiverTask(__attribute__((unused)) void *parameters) /* Reset the aging timer because activity was detected */ lastActivityTime = lastSysTime; } + /* Read signal quality from the group used for the throttle */ + (void)updateRcvrStatus(&activity_fsm, + settings.ChannelGroups.Throttle); } if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { resetRcvrActivity(&activity_fsm); + resetRcvrStatus(&activity_fsm); lastActivityTime = lastSysTime; } @@ -278,6 +290,10 @@ static void receiverTask(__attribute__((unused)) void *parameters) } } + /* Read signal quality from the group used for the throttle */ + (void)updateRcvrStatus(&activity_fsm, + settings.ChannelGroups.Throttle); + // Sanity Check Throttle and Yaw if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE @@ -568,6 +584,12 @@ static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) fsm->sample_count = 0; } +static void resetRcvrStatus(struct rcvr_activity_fsm *fsm) +{ + /* Reset the state */ + fsm->quality = 0; +} + static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { for (uint8_t channel = 1; channel <= max_channels; channel++) { @@ -641,6 +663,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { /* We're out of range, reset things */ resetRcvrActivity(fsm); + resetRcvrStatus(fsm); } extern uint32_t pios_rcvr_group_map[]; @@ -686,6 +709,32 @@ group_completed: return activity_updated; } +/* Read signal quality from the specified group */ +static bool updateRcvrStatus( + struct rcvr_activity_fsm *fsm, + ManualControlSettingsChannelGroupsOptions group) +{ + extern uint32_t pios_rcvr_group_map[]; + bool activity_updated = false; + int8_t quality; + + quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]); + + /* If no driver is detected or any other error then return */ + if (quality < 0) { + return activity_updated; + } + + /* Compare with previous sample */ + if (quality != fsm->quality) { + fsm->quality = quality; + ReceiverStatusQualitySet(&fsm->quality); + activity_updated = true; + } + + return activity_updated; +} + /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.cpp similarity index 52% rename from flight/modules/Stabilization/altitudeloop.c rename to flight/modules/Stabilization/altitudeloop.cpp index 489e6e0d1..dab918ae4 100644 --- a/flight/modules/Stabilization/altitudeloop.c +++ b/flight/modules/Stabilization/altitudeloop.cpp @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file altitudeloop.c + * @file altitudeloop.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field @@ -26,6 +26,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +extern "C" { #include #include @@ -37,6 +38,12 @@ #include #include #include +#include +#include +} + +#include + // Private constants @@ -50,78 +57,134 @@ #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define STACK_SIZE_BYTES 512 // Private types // Private variables static DelayedCallbackInfo *altitudeHoldCBInfo; +static PIDControlDown controlDown; static AltitudeHoldSettingsData altitudeHoldSettings; -static struct pid pid0, pid1; static ThrustModeType thrustMode; -static PiOSDeltatimeConfig timeval; -static float thrustSetpoint = 0.0f; -static float thrustDemand = 0.0f; -static float startThrust = 0.5f; +static float thrustDemand = 0.0f; // Private functions -static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +static void altitudeHoldTask(void); static void VelocityStateUpdatedCb(UAVObjEvent *ev); /** * Setup mode and setpoint + * + * reinit: true when althold/vario mode selected over a previous alternate thrust mode */ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit) { static bool newaltitude = true; - if (reinit) { - startThrust = setpoint; - pid_zero(&pid0); - pid_zero(&pid1); + if (reinit || !controlDown.IsActive()) { + controlDown.Activate(); newaltitude = true; + // calculate a thrustDemand on reinit only + altitudeHoldTask(); } const float DEADBAND = 0.20f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - // this is the max speed in m/s at the extents of thrust - float thrustRate; - uint8_t thrustExp; - - AltitudeHoldSettingsThrustExpGet(&thrustExp); - AltitudeHoldSettingsThrustRateGet(&thrustRate); - - PositionStateData posState; - PositionStateGet(&posState); if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) { // Cut thrust if desired - thrustSetpoint = 0.0f; - thrustDemand = 0.0f; - thrustMode = DIRECT; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(0.0f); + thrustDemand = 0.0f; + thrustMode = DIRECT; + newaltitude = true; + return thrustDemand; } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); - thrustMode = ALTITUDEVARIO; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate)); + thrustMode = ALTITUDEVARIO; + newaltitude = true; } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) { - thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate); - thrustMode = ALTITUDEVARIO; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate)); + thrustMode = ALTITUDEVARIO; + newaltitude = true; } else if (newaltitude == true) { - thrustSetpoint = posState.Down; - thrustMode = ALTITUDEHOLD; - newaltitude = false; + controlDown.UpdateVelocitySetpoint(0.0f); + PositionStateData posState; + PositionStateGet(&posState); + controlDown.UpdatePositionSetpoint(posState.Down); + thrustMode = ALTITUDEHOLD; + newaltitude = false; } + thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max); + return thrustDemand; } +/** + * Disable the alt hold task loop velocity controller to save cpu cycles + */ +void stabilizationDisableAltitudeHold(void) +{ + controlDown.Deactivate(); +} + + +/** + * Module thread, should not return. + */ +static void altitudeHoldTask(void) +{ + if (!controlDown.IsActive()) { + return; + } + + AltitudeHoldStatusData altitudeHoldStatus; + AltitudeHoldStatusGet(&altitudeHoldStatus); + + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); + controlDown.UpdateVelocityState(velocityStateDown); + + float local_thrustDemand = 0.0f; + + switch (thrustMode) { + case ALTITUDEHOLD: + { + float positionStateDown; + PositionStateDownGet(&positionStateDown); + controlDown.UpdatePositionState(positionStateDown); + controlDown.ControlPosition(); + altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD; + local_thrustDemand = controlDown.GetDownCommand(); + } + break; + + case ALTITUDEVARIO: + altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO; + local_thrustDemand = controlDown.GetDownCommand(); + break; + + case DIRECT: + altitudeHoldStatus.VelocityDesired = 0.0f; + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT; + break; + } + + AltitudeHoldStatusSet(&altitudeHoldStatus); + thrustDemand = local_thrustDemand; +} + +static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); +} + /** * Initialise the module, called on startup */ @@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit() AltitudeHoldStatusInitialize(); PositionStateInitialize(); VelocityStateInitialize(); + VtolSelfTuningStatsInitialize(); - PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); - // Create object queue + AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb); + SettingsUpdatedCb(NULL); altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); - AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); VelocityStateConnectCallback(&VelocityStateUpdatedCb); - - // Start main task - SettingsUpdatedCb(NULL); } -/** - * Module thread, should not return. - */ -static void altitudeHoldTask(void) -{ - AltitudeHoldStatusData altitudeHoldStatus; - - AltitudeHoldStatusGet(&altitudeHoldStatus); - - // do the actual control loop(s) - float positionStateDown; - PositionStateDownGet(&positionStateDown); - float velocityStateDown; - VelocityStateDownGet(&velocityStateDown); - - float dT; - dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - switch (thrustMode) { - case ALTITUDEHOLD: - { - // altitude control loop - // No scaling. - const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT); - } - break; - case ALTITUDEVARIO: - altitudeHoldStatus.VelocityDesired = thrustSetpoint; - break; - default: - altitudeHoldStatus.VelocityDesired = 0; - break; - } - - AltitudeHoldStatusSet(&altitudeHoldStatus); - - switch (thrustMode) { - case DIRECT: - thrustDemand = thrustSetpoint; - break; - default: - { - // velocity control loop - // No scaling. - const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; - thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); - } - break; - } -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit); - pid_zero(&pid0); - pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit); - pid_zero(&pid1); -} -static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); + controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp, + altitudeHoldSettings.VerticalVelPID.Ki, + altitudeHoldSettings.VerticalVelPID.Kd, + altitudeHoldSettings.VerticalVelPID.Beta, + (float)(UPDATE_EXPECTED), + altitudeHoldSettings.ThrustRate); + + controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP); + + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral); + + // initialise limits on thrust but note the FSM can override. + controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max); + + // disable neutral thrust calcs which should only be done in a hold mode. + controlDown.DisableNeutralThrustCalc(); } diff --git a/flight/modules/Stabilization/inc/altitudeloop.h b/flight/modules/Stabilization/inc/altitudeloop.h index d4789c6eb..f29709149 100644 --- a/flight/modules/Stabilization/inc/altitudeloop.h +++ b/flight/modules/Stabilization/inc/altitudeloop.h @@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType; void stabilizationAltitudeloopInit(); float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit); +void stabilizationDisableAltitudeHold(void); #endif /* ALTITUDELOOP_H */ diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 7771591c9..1fb2a4061 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -103,7 +103,33 @@ static void stabilizationOuterloopTask() float *stabilizationDesiredAxis = &stabilizationDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; int t; - float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]); + previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]; +#ifdef REVOLUTION + if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE || + previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) { + // disable the altvario velocity control loop + stabilizationDisableAltitudeHold(); + } +#endif /* REVOLUTION */ + switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) { +#ifdef REVOLUTION + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit); + break; + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit); + break; +#endif /* REVOLUTION */ + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: + default: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST]; + break; + } + float local_error[3]; { @@ -151,151 +177,134 @@ static void stabilizationOuterloopTask() } - for (t = 0; t < AXES; t++) { - bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); + for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) { + reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t]; - if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { - if (reinit) { - stabSettings.outerPids[t].iAccumulator = 0; - } - switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { - case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: - rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); - break; - case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: - { - float stickinput[3]; - stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); - stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); - stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); - float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; - // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together - rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), - -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t], - StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t] - ); - // Compute the weighted average rate desired - // Using max() rather than sqrt() for cpu speed; - // - this makes the stick region into a square; - // - this is a feature! - // - hold a roll angle and add just pitch without the stick sensitivity changing - float magnitude = fabsf(stickinput[t]); - if (t < 2) { - magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); - } - - // modify magnitude to move the Att to Rate transition to the place - // specified by the user - // we are looking for where the stick angle == transition angle - // and the Att rate equals the Rate rate - // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] - // == Rate x StickAngle [Rate pulling up according to stick angle] - // * StickAngle [X Ratt proportion] - // so 1-x == x*x or x*x+x-1=0 where xE(0,1) - // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 - // and quadratic formula says that is 0.618033989f - // I tested 14.01 and came up with .61 without even remembering this number - // I thought that moving the P,I, and maxangle terms around would change this value - // and that I would have to take these into account, but varying - // all P's and I's by factors of 1/2 to 2 didn't change it noticeably - // and varying maxangle from 4 to 120 didn't either. - // so for now I'm not taking these into account - // while working with this, it occurred to me that Attitude mode, - // set up with maxangle=190 would be similar to Ratt, and it is. - #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f - - // the following assumes the transition would otherwise be at 0.618033989f - // and THAT assumes that Att ramps up to max roll rate - // when a small number of degrees off of where it should be - - // if below the transition angle (still in attitude mode) - // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ - if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { - magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; - } else { - magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) - * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) - / (1.0f - stabSettings.rattitude_mode_transition_stick_position) - + STICK_VALUE_AT_MODE_TRANSITION; - } - rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; - } + if (reinit) { + stabSettings.outerPids[t].iAccumulator = 0; + } + switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { + case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: + rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); break; - case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: - // FIXME: local_error[] is rate - attitude for Weak Leveling - // The only ramifications are: - // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS - // Changing Rate mode max rate currently requires a change to Kp - // That would be changed to Attitude mode max angle affecting Kp - // Also does not take dT into account - { - float stickinput[3]; - stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); - stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); - stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); - float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; - float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; - weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); - - // Compute desired rate as input biased towards leveling - rateDesiredAxis[t] = rate_input + weak_leveling; + case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; + // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together + rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), + -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t], + StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t] + ); + // Compute the weighted average rate desired + // Using max() rather than sqrt() for cpu speed; + // - this makes the stick region into a square; + // - this is a feature! + // - hold a roll angle and add just pitch without the stick sensitivity changing + float magnitude = fabsf(stickinput[t]); + if (t < 2) { + magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); } - break; - case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes - // now test limits for pitch and/or roll - if (t == 1) { // pitch - if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { - // attitude exceeds pitch max. - // zero rate desired if also -ve - if (rateDesiredAxis[t] < 0.0f) { - rateDesiredAxis[t] = 0.0f; - } - } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { - // attitude exceeds pitch max - // zero rate desired if also +ve - if (rateDesiredAxis[t] > 0.0f) { - rateDesiredAxis[t] = 0.0f; - } + + // modify magnitude to move the Att to Rate transition to the place + // specified by the user + // we are looking for where the stick angle == transition angle + // and the Att rate equals the Rate rate + // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] + // == Rate x StickAngle [Rate pulling up according to stick angle] + // * StickAngle [X Ratt proportion] + // so 1-x == x*x or x*x+x-1=0 where xE(0,1) + // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 + // and quadratic formula says that is 0.618033989f + // I tested 14.01 and came up with .61 without even remembering this number + // I thought that moving the P,I, and maxangle terms around would change this value + // and that I would have to take these into account, but varying + // all P's and I's by factors of 1/2 to 2 didn't change it noticeably + // and varying maxangle from 4 to 120 didn't either. + // so for now I'm not taking these into account + // while working with this, it occurred to me that Attitude mode, + // set up with maxangle=190 would be similar to Ratt, and it is. +#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f + + // the following assumes the transition would otherwise be at 0.618033989f + // and THAT assumes that Att ramps up to max roll rate + // when a small number of degrees off of where it should be + + // if below the transition angle (still in attitude mode) + // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { + magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; + } else { + magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) + * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) + / (1.0f - stabSettings.rattitude_mode_transition_stick_position) + + STICK_VALUE_AT_MODE_TRANSITION; + } + rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: + // FIXME: local_error[] is rate - attitude for Weak Leveling + // The only ramifications are: + // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS + // Changing Rate mode max rate currently requires a change to Kp + // That would be changed to Attitude mode max angle affecting Kp + // Also does not take dT into account + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; + float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; + weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); + + // Compute desired rate as input biased towards leveling + rateDesiredAxis[t] = rate_input + weak_leveling; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes + // now test limits for pitch and/or roll + if (t == 1) { // pitch + if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; } - } else if (t == 0) { // roll - if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { - // attitude exceeds roll max. - // zero rate desired if also -ve - if (rateDesiredAxis[t] < 0.0f) { - rateDesiredAxis[t] = 0.0f; - } - } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { - // attitude exceeds roll max - // zero rate desired if also +ve - if (rateDesiredAxis[t] > 0.0f) { - rateDesiredAxis[t] = 0.0f; - } + } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; } } - break; + } else if (t == 0) { // roll + if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { + // attitude exceeds roll max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { + // attitude exceeds roll max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } + } + break; - case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: - default: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; - break; - } - } else { - switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { -#ifdef REVOLUTION - case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: - rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit); - break; - case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: - rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit); - break; -#endif /* REVOLUTION */ - case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: - default: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; - break; - } + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; } } @@ -314,7 +323,7 @@ static void stabilizationOuterloopTask() } } - // update cruisecontrol based on attitude +// update cruisecontrol based on attitude cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust); stabSettings.monitor.rateupdates = 0; } diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index c4e94fbc8..7b6b7b2ee 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -55,6 +55,9 @@ #include "accessorydesired.h" #include "manualcontrolcommand.h" #include "stabilizationsettings.h" +#ifdef REVOLUTION +#include "altitudeholdsettings.h" +#endif #include "stabilizationbank.h" #include "stabilizationsettingsbank1.h" #include "stabilizationsettingsbank2.h" @@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void) bool txPIDEnabled; HwSettingsOptionalModulesData optionalModules; +#ifdef REVOLUTION + AltitudeHoldSettingsInitialize(); +#endif + HwSettingsInitialize(); HwSettingsOptionalModulesGet(&optionalModules); @@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev) } StabilizationSettingsData stab; StabilizationSettingsGet(&stab); +#ifdef REVOLUTION + AltitudeHoldSettingsData altitude; + AltitudeHoldSettingsGet(&altitude); +#endif AccessoryDesiredData accessory; - uint8_t needsUpdateBank = 0; - uint8_t needsUpdateStab = 0; + uint8_t needsUpdateBank = 0; + uint8_t needsUpdateStab = 0; +#ifdef REVOLUTION + uint8_t needsUpdateAltitude = 0; +#endif // Loop through every enabled instance for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { @@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR: needsUpdateBank |= update(&bank.AcroInsanityFactor, value); break; +#ifdef REVOLUTION + case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP: + needsUpdateAltitude |= update(&altitude.VerticalPosP, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value); + break; +#endif default: PIOS_Assert(0); } @@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev) if (needsUpdateStab) { StabilizationSettingsSet(&stab); } +#ifdef REVOLUTION + if (needsUpdateAltitude) { + AltitudeHoldSettingsSet(&altitude); + } +#endif if (needsUpdateBank) { switch (inst.BankNumber) { case 0: diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 07e703ba2..9a4b83f60 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -283,6 +283,56 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } +/** + * Set control lines associated with the port + * \param[in] port COM port + * \param[in] mask Lines to change + * \param[in] state New state for lines + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_ctrl_line) { + com_dev->driver->set_ctrl_line(com_dev->lower_id, mask, state); + } + + return 0; +} + +/** + * Set control lines associated with the port + * \param[in] port COM port + * \param[in] ctrl_line_cb Callback function + * \param[in] context context to pass to the callback function + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t com_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + /* Invoke the driver function if it exists */ + if (com_dev->driver->bind_ctrl_line_cb) { + com_dev->driver->bind_ctrl_line_cb(com_dev->lower_id, ctrl_line_cb, context); + } + + return 0; +} + static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len) { diff --git a/flight/pios/common/pios_mpxv.c b/flight/pios/common/pios_mpxv.c index 4e6e3ce82..dc08d6ed2 100644 --- a/flight/pios/common/pios_mpxv.c +++ b/flight/pios/common/pios_mpxv.c @@ -68,8 +68,8 @@ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement) */ float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement) { - // Calculate dynamic pressure, as per docs - float Qc = 3.3f / 4096.0f * (float)(measurement - desc->zeroPoint); + // Calculate dynamic pressure, as per docs - Apply scale factor (voltage divider) + float Qc = 3.3f / 4096.0f * (float)((measurement - desc->zeroPoint) * desc->scale); // Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. diff --git a/flight/pios/common/pios_oplinkrcvr.c b/flight/pios/common/pios_oplinkrcvr.c index c4579a0b5..5c967c105 100644 --- a/flight/pios/common/pios_oplinkrcvr.c +++ b/flight/pios/common/pios_oplinkrcvr.c @@ -34,6 +34,7 @@ #include #include +#include #include static OPLinkReceiverData oplinkreceiverdata; @@ -41,9 +42,11 @@ static OPLinkReceiverData oplinkreceiverdata; /* Provide a RCVR driver */ static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_oplinkrcvr_Supervisor(uint32_t ppm_id); +static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id); const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver = { - .read = PIOS_OPLinkRCVR_Get, + .read = PIOS_OPLinkRCVR_Get, + .get_quality = PIOS_OPLinkRCVR_Quality_Get }; /* Local Variables */ @@ -186,6 +189,16 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id) oplinkrcvr_dev->Fresh = false; } +static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id) +{ + uint8_t oplink_quality; + + OPLinkStatusLinkQualityGet(&oplink_quality); + + /* link_status is in the range 0-128, so scale to a % */ + return oplink_quality * 100 / 128; +} + #endif /* PIOS_INCLUDE_OPLINKRCVR */ /** diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index b947c5142..afae44b2a 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -113,6 +113,34 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } +/** + * @brief Reads input quality from the appropriate driver + * @param[in] rcvr_id driver to read from + * @returns received signal quality expressed as a % + * @retval PIOS_RCVR_NODRIVER driver was not initialized + */ +uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id) +{ + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } + + struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; + + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + /* As no receiver is available assume min */ + return 0; + } + + if (!rcvr_dev->driver->get_quality) { + /* If no quality is available assume max */ + return 100; + } + + return rcvr_dev->driver->get_quality(rcvr_dev->lower_id); +} + /** * @brief Get a semaphore that signals when a new sample is available. * @param[in] rcvr_id driver to read from diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 69089e9cd..e354a5c5f 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -32,6 +32,10 @@ #ifdef PIOS_INCLUDE_SBUS +// Define to report number of frames since last dropped instead of weighted ave +#undef SBUS_GOOD_FRAME_COUNT + +#include #include "pios_sbus_priv.h" /* Forward Declarations */ @@ -42,11 +46,12 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context, uint16_t *headroom, bool *need_yield); static void PIOS_SBus_Supervisor(uint32_t sbus_id); - +static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id); /* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { - .read = PIOS_SBus_Get, + .read = PIOS_SBus_Get, + .get_quality = PIOS_SBus_Quality_Get }; enum pios_sbus_dev_magic { @@ -60,8 +65,17 @@ struct pios_sbus_state { uint8_t failsafe_timer; uint8_t frame_found; uint8_t byte_count; + float quality; +#ifdef SBUS_GOOD_FRAME_COUNT + uint8_t frame_count; +#endif /* SBUS_GOOD_FRAME_COUNT */ }; +/* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples + * gives about a 200ms response. + */ +#define SBUS_FL_WEIGHTED_AVE 26 + struct pios_sbus_dev { enum pios_sbus_dev_magic magic; const struct pios_sbus_cfg *cfg; @@ -120,6 +134,10 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state) state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; + state->quality = 0.0f; +#ifdef SBUS_GOOD_FRAME_COUNT + state->frame_count = 0; +#endif /* SBUS_GOOD_FRAME_COUNT */ PIOS_SBus_ResetChannels(state); } @@ -251,18 +269,42 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) state->byte_count++; } else { if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) { +#ifndef SBUS_GOOD_FRAME_COUNT + /* Quality trend is towards 0% by default*/ + uint8_t quality_trend = 0; +#endif /* SBUS_GOOD_FRAME_COUNT */ + /* full frame received */ uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; if (flags & SBUS_FLAG_FL) { /* frame lost, do not update */ - } else if (flags & SBUS_FLAG_FS) { - /* failsafe flag active */ - PIOS_SBus_ResetChannels(state); +#ifdef SBUS_GOOD_FRAME_COUNT + state->quality = state->frame_count; + state->frame_count = 0; +#endif /* SBUS_GOOD_FRAME_COUNT */ } else { - /* data looking good */ - PIOS_SBus_UnrollChannels(state); - state->failsafe_timer = 0; +#ifdef SBUS_GOOD_FRAME_COUNT + if (++state->frame_count == 255) { + state->quality = state->frame_count--; + } +#else /* SBUS_GOOD_FRAME_COUNT */ + /* Quality trend is towards 100% */ + quality_trend = 100; +#endif /* SBUS_GOOD_FRAME_COUNT */ + if (flags & SBUS_FLAG_FS) { + /* failsafe flag active */ + PIOS_SBus_ResetChannels(state); + } else { + /* data looking good */ + PIOS_SBus_UnrollChannels(state); + state->failsafe_timer = 0; + } } +#ifndef SBUS_GOOD_FRAME_COUNT + /* Present quality as a weighted average of good frames */ + state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) + + quality_trend) / SBUS_FL_WEIGHTED_AVE; +#endif /* SBUS_GOOD_FRAME_COUNT */ } else { /* discard whole frame */ } @@ -341,6 +383,19 @@ static void PIOS_SBus_Supervisor(uint32_t sbus_id) } } +static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id) +{ + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; + + bool valid = PIOS_SBus_Validate(sbus_dev); + + PIOS_Assert(valid); + + struct pios_sbus_state *state = &(sbus_dev->state); + + return (uint8_t)(state->quality + 0.5f); +} + #endif /* PIOS_INCLUDE_SBUS */ /** diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index e2a9639c5..b2591e4f0 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -8,6 +8,7 @@ * * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * @@ -35,19 +36,29 @@ #include /* bool */ typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); +typedef void (*pios_com_callback_ctrl_line)(uint32_t context, uint32_t mask, uint32_t state); struct pios_com_driver { void (*init)(uint32_t id); void (*set_baud)(uint32_t id, uint32_t baud); + void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state); void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + void (*bind_ctrl_line_cb)(uint32_t id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); bool (*available)(uint32_t id); }; +/* Control line definitions */ +#define COM_CTRL_LINE_DTR_MASK 0x01 +#define COM_CTRL_LINE_RTS_MASK 0x02 + /* Public Functions */ +extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); +extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); +extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len); diff --git a/flight/pios/inc/pios_mpxv.h b/flight/pios/inc/pios_mpxv.h index 6b60eacdf..842e66335 100644 --- a/flight/pios/inc/pios_mpxv.h +++ b/flight/pios/inc/pios_mpxv.h @@ -38,6 +38,7 @@ typedef struct { uint16_t calibrationCount; uint32_t calibrationSum; uint16_t zeroPoint; + float scale; float maxSpeed; } PIOS_MPXV_descriptor; diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h index 74fac086f..c289635d0 100644 --- a/flight/pios/inc/pios_rcvr.h +++ b/flight/pios/inc/pios_rcvr.h @@ -35,10 +35,12 @@ struct pios_rcvr_driver { void (*init)(uint32_t id); int32_t (*read)(uint32_t id, uint8_t channel); xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel); + uint8_t (*get_quality)(uint32_t id); }; /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); +extern uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id); extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ diff --git a/flight/pios/inc/pios_usart_priv.h b/flight/pios/inc/pios_usart_priv.h index 52394e5f4..68a44bc9d 100644 --- a/flight/pios/inc/pios_usart_priv.h +++ b/flight/pios/inc/pios_usart_priv.h @@ -44,6 +44,7 @@ struct pios_usart_cfg { USART_InitTypeDef init; struct stm32_gpio rx; struct stm32_gpio tx; + struct stm32_gpio dtr; struct stm32_irq irq; }; diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index 164d31fb3..9078e1abb 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -353,7 +353,6 @@ enum usb_cdc_notification { enum usb_product_ids { USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, - USB_PRODUCT_ID_COPTERCONTROL = 0x415B, USB_PRODUCT_ID_OPLINK = 0x415C, USB_PRODUCT_ID_CC3D = 0x415D, USB_PRODUCT_ID_REVOLUTION = 0x415E, @@ -364,9 +363,9 @@ enum usb_product_ids { enum usb_op_board_ids { USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, /* Board ID 2 may be unused or AHRS */ - USB_OP_BOARD_ID_OPLINK = 3, - USB_OP_BOARD_ID_COPTERCONTROL = 4, - USB_OP_BOARD_ID_REVOLUTION = 5, + USB_OP_BOARD_ID_OPLINK = 3, + /* USB_OP_BOARD_ID_COPTERCONTROL = 4, */ + USB_OP_BOARD_ID_REVOLUTION = 5, USB_OP_BOARD_ID_OSD = 6, } __attribute__((packed)); diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c index 0a3450a9f..44fa484be 100644 --- a/flight/pios/stm32f10x/pios_dsm.c +++ b/flight/pios/stm32f10x/pios_dsm.c @@ -34,9 +34,12 @@ #include "pios_dsm_priv.h" +// *** UNTESTED CODE *** +#undef DSM_LINK_QUALITY /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); +static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, @@ -46,7 +49,8 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, + .get_quality = PIOS_DSM_Quality_Get }; enum pios_dsm_dev_magic { @@ -60,12 +64,15 @@ struct pios_dsm_state { uint8_t failsafe_timer; uint8_t frame_found; uint8_t byte_count; -#ifdef DSM_LOST_FRAME_COUNTER uint8_t frames_lost_last; - uint16_t frames_lost; -#endif + float quality; }; +/* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples + * gives about a 200ms response. + */ +#define DSM_FL_WEIGHTED_AVE 18 + struct pios_dsm_dev { enum pios_dsm_dev_magic magic; const struct pios_dsm_cfg *cfg; @@ -164,10 +171,8 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; -#ifdef DSM_LOST_FRAME_COUNTER + state->quality = 0.0f; state->frames_lost_last = 0; - state->frames_lost = 0; -#endif PIOS_DSM_ResetChannels(dsm_dev); } @@ -183,13 +188,24 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) static uint8_t resolution = 11; uint32_t channel_log = 0; -#ifdef DSM_LOST_FRAME_COUNTER + // *** UNTESTED CODE *** +#ifdef DSM_LINK_QUALITY /* increment the lost frame counter */ uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); - state->frames_lost_last = frames_lost; -#endif + /* We only get a lost frame count when the next good frame comes in */ + /* Present quality as a weighted average of good frames */ + /* First consider the bad frames */ + for (int i = 0; i < frames_lost - state->frames_lost_last; i++) { + state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) / + DSM_FL_WEIGHTED_AVE; + } + /* And now the good frame */ + state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) + + 100) / DSM_FL_WEIGHTED_AVE; + + state->frames_lost_last = frames_lost; +#endif /* DSM_LINK_QUALITY */ /* unroll channels */ uint8_t *s = &(state->received_data[2]); @@ -238,11 +254,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) } } -#ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; -#endif - /* all channels processed */ return 0; @@ -406,6 +417,19 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id) } } +static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id) +{ + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + + bool valid = PIOS_DSM_Validate(dsm_dev); + + PIOS_Assert(valid); + + struct pios_dsm_state *state = &(dsm_dev->state); + + return (uint8_t)(state->quality + 0.5f); +} + #endif /* PIOS_INCLUDE_DSM */ /** diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index 055f9505c..bdd8244e1 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -34,12 +34,16 @@ #include "pios_dsm_priv.h" +// *** UNTESTED CODE *** +#undef DSM_LINK_QUALITY + #ifndef PIOS_INCLUDE_RTC #error PIOS_INCLUDE_RTC must be used to use DSM #endif /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); +static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, @@ -49,7 +53,8 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, + .get_quality = PIOS_DSM_Quality_Get }; enum pios_dsm_dev_magic { @@ -63,12 +68,15 @@ struct pios_dsm_state { uint8_t failsafe_timer; uint8_t frame_found; uint8_t byte_count; -#ifdef DSM_LOST_FRAME_COUNTER uint8_t frames_lost_last; - uint16_t frames_lost; -#endif + float quality; }; +/* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples + * gives about a 200ms response. + */ +#define DSM_FL_WEIGHTED_AVE 18 + struct pios_dsm_dev { enum pios_dsm_dev_magic magic; const struct pios_dsm_cfg *cfg; @@ -167,10 +175,8 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; -#ifdef DSM_LOST_FRAME_COUNTER + state->quality = 0.0f; state->frames_lost_last = 0; - state->frames_lost = 0; -#endif PIOS_DSM_ResetChannels(dsm_dev); } @@ -186,12 +192,24 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) static uint8_t resolution = 11; uint32_t channel_log = 0; -#ifdef DSM_LOST_FRAME_COUNTER + // *** UNTESTED CODE *** +#ifdef DSM_LINK_QUALITY /* increment the lost frame counter */ uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); + + /* We only get a lost frame count when the next good frame comes in */ + /* Present quality as a weighted average of good frames */ + /* First consider the bad frames */ + for (int i = 0; i < frames_lost - state->frames_lost_last; i++) { + state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) / + DSM_FL_WEIGHTED_AVE; + } + /* And now the good frame */ + state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) + + 100) / DSM_FL_WEIGHTED_AVE; + state->frames_lost_last = frames_lost; -#endif +#endif /* DSM_LINK_QUALITY */ /* unroll channels */ uint8_t *s = &(state->received_data[2]); @@ -240,11 +258,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) } } -#ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; -#endif - /* all channels processed */ return 0; @@ -408,6 +421,19 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id) } } +static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id) +{ + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + + bool valid = PIOS_DSM_Validate(dsm_dev); + + PIOS_Assert(valid); + + struct pios_dsm_state *state = &(dsm_dev->state); + + return (uint8_t)(state->quality + 0.5f); +} + #endif /* PIOS_INCLUDE_DSM */ /** diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 3a41b08fd..bcc26cacf 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -40,17 +40,19 @@ /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); +static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state); static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .set_ctrl_line = PIOS_USART_SetCtrlLine, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { @@ -189,6 +191,12 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + /* If a DTR line is specified, initialize it */ + if (usart_dev->cfg->dtr.gpio) { + GPIO_Init(usart_dev->cfg->dtr.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->dtr.init); + PIOS_USART_SetCtrlLine((uint32_t)usart_dev, COM_CTRL_LINE_DTR_MASK, 0); + } + /* Configure the USART */ USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); @@ -276,6 +284,22 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } +static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* Only attempt to drive DTR if this USART has a GPIO line defined */ + if (usart_dev->cfg->dtr.gpio && (mask & COM_CTRL_LINE_DTR_MASK)) { + GPIO_WriteBit(usart_dev->cfg->dtr.gpio, + usart_dev->cfg->dtr.init.GPIO_Pin, + state & COM_CTRL_LINE_DTR_MASK ? Bit_RESET : Bit_SET); + } +} + static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; diff --git a/flight/pios/stm32f4xx/pios_usb_cdc.c b/flight/pios/stm32f4xx/pios_usb_cdc.c index 253295d94..277165af2 100644 --- a/flight/pios/stm32f4xx/pios_usb_cdc.c +++ b/flight/pios/stm32f4xx/pios_usb_cdc.c @@ -40,6 +40,7 @@ /* Implement COM layer driver API */ static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterCtrlLineCallback(uint32_t usbcdc_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); static bool PIOS_USB_CDC_Available(uint32_t usbcdc_id); @@ -49,6 +50,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = { .rx_start = PIOS_USB_CDC_RxStart, .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .bind_ctrl_line_cb = PIOS_USB_CDC_RegisterCtrlLineCallback, .available = PIOS_USB_CDC_Available, }; @@ -66,6 +68,8 @@ struct pios_usb_cdc_dev { uint32_t rx_in_context; pios_com_callback tx_out_cb; uint32_t tx_out_context; + pios_com_callback_ctrl_line ctrl_line_cb; + uint32_t ctrl_line_context; bool usb_ctrl_if_enabled; bool usb_data_if_enabled; @@ -323,6 +327,23 @@ static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callbac usb_cdc_dev->tx_out_cb = tx_out_cb; } +static void PIOS_USB_CDC_RegisterCtrlLineCallback(uint32_t usbcdc_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->ctrl_line_context = context; + usb_cdc_dev->ctrl_line_cb = ctrl_line_cb; +} + + static bool PIOS_USB_CDC_CTRL_EP_IN_Callback(uint32_t usb_cdc_id, uint8_t epnum, uint16_t len); static void PIOS_USB_CDC_CTRL_IF_Init(uint32_t usb_cdc_id) @@ -403,6 +424,11 @@ static bool PIOS_USB_CDC_CTRL_IF_Setup(uint32_t usb_cdc_id, struct usb_setup_req break; case USB_CDC_REQ_SET_CONTROL_LINE_STATE: control_line_state = req->wValue; + if (usb_cdc_dev->ctrl_line_cb) { + (usb_cdc_dev->ctrl_line_cb)(usb_cdc_dev->ctrl_line_context, + 0xff, + control_line_state); + } break; default: /* Unhandled class request */ diff --git a/flight/targets/boards/coptercontrol/board-info.mk b/flight/targets/boards/coptercontrol/board-info.mk deleted file mode 100644 index d58f3d71d..000000000 --- a/flight/targets/boards/coptercontrol/board-info.mk +++ /dev/null @@ -1,23 +0,0 @@ -BOARD_TYPE := 0x04 -BOARD_REVISION := 0x02 -BOOTLOADER_VERSION := 0x04 -HW_TYPE := 0x01 - -MCU := cortex-m3 -CHIP := STM32F103CBT -BOARD := STM32103CB_CC_Rev1 -MODEL := MD -MODEL_SUFFIX := _CC - -OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg -OPENOCD_CONFIG := stm32f1x.cfg - -# Note: These must match the values in link_$(BOARD)_memory.ld -BL_BANK_BASE := 0x08000000 # Start of bootloader flash -BL_BANK_SIZE := 0x00003000 # Should include BD_INFO region -FW_BANK_BASE := 0x08003000 # Start of firmware flash -FW_BANK_SIZE := 0x0001D000 # Should include FW_DESC_SIZE - -FW_DESC_SIZE := 0x00000064 - -OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c deleted file mode 100644 index 17981095f..000000000 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ /dev/null @@ -1,1525 +0,0 @@ -/** - ****************************************************************************** - * @file board_hw_defs.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @{ - * @brief Defines board specific static initializers for hardware for the CopterControl board. - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#define BOARD_REVISION_CC 1 -#define BOARD_REVISION_CC3D 2 - -#if defined(PIOS_INCLUDE_LED) - -#include -static const struct pios_gpio pios_leds_cc[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - .active_low = true - }, -}; - -static const struct pios_gpio_cfg pios_led_cfg_cc = { - .gpios = pios_leds_cc, - .num_gpios = NELEMENTS(pios_leds_cc), -}; - -static const struct pios_gpio pios_leds_cc3d[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - .remap = GPIO_Remap_SWJ_JTAGDisable, - .active_low = true - }, -}; - -static const struct pios_gpio_cfg pios_led_cfg_cc3d = { - .gpios = pios_leds_cc3d, - .num_gpios = NELEMENTS(pios_leds_cc3d), -}; - -const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) -{ - switch (board_revision) { - case BOARD_REVISION_CC: return &pios_led_cfg_cc; - - case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; - - default: return NULL; - } -} - -#endif /* PIOS_INCLUDE_LED */ - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* Gyro interface */ -void PIOS_SPI_gyro_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); -static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 1, - .ssel = { - { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - } - }, -}; - -static uint32_t pios_spi_gyro_id; -void PIOS_SPI_gyro_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); -} - - -/* Flash/Accel Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). - */ -void PIOS_SPI_flash_accel_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); -static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = { - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - } - },{ - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - } - }, -}; - -static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = { - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - } - },{ - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - } - }, -}; - -static uint32_t pios_spi_flash_accel_id; -void PIOS_SPI_flash_accel_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -#if defined(PIOS_INCLUDE_FLASH) -#include "pios_flashfs_logfs_priv.h" -#include "pios_flash_jedec_priv.h" - -static const struct flashfs_logfs_cfg flashfs_w25x_cfg = { - .fs_magic = 0x99abcdef, - .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ - - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00001000, /* 4K bytes */ - .page_size = 0x00000100, /* 256 bytes */ -}; - - -static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ - - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ -}; - -#include "pios_flash.h" - -#endif /* PIOS_INCLUDE_FLASH */ - -/* - * ADC system - */ -#if defined(PIOS_INCLUDE_ADC) -#include "pios_adc_priv.h" -extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); -// Remap the ADC DMA handler to this one -static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, -}; - -void PIOS_ADC_handler() -{ - PIOS_ADC_DMA_Handler(); -} -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#include "pios_tim_priv.h" - -static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, -}; - -static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .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, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, -}; - -static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, -}; - - -static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - - // Receiver port pins - // S3-S6 inputs are used as outputs in this case - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, -}; - -static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, -}; - -#if defined(PIOS_INCLUDE_USART) - -#include "pios_usart_priv.h" - -static const struct pios_usart_cfg pios_usart_generic_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_generic_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, - }, - }, -}; - -#if defined(PIOS_INCLUDE_DSM) -/* - * Spektrum/JR DSM USART - */ -#include - -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .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, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .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_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .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, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .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_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - - -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .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_IN_FLOATING, - }, - }, -}; - -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_clk_func = RCC_APB2PeriphClockCmd, - .gpio_clk_periph = RCC_APB2Periph_GPIOB, - .gpio_inv_enable = Bit_SET, -}; - -#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) - -#include "pios_com_priv.h" - -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_RTC) -/* - * Realtime Clock (RTC) - */ -#include - -void PIOS_RTC_IRQ_Handler(void); -void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); -static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -void PIOS_RTC_IRQ_Handler(void) -{ - PIOS_RTC_irq_handler(); -} - -#endif /* if defined(PIOS_INCLUDE_RTC) */ - -#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) -/* - * Servo outputs - */ -#include - -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 = PIOS_SERVOS_INITIAL_POSITION, - .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), -}; - -const struct pios_servo_cfg pios_servo_rcvr_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_rcvrport_pins, - .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), -}; - -#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ - -/* - * PPM Inputs - */ -#if defined(PIOS_INCLUDE_PPM) -#include - -const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, -}; - -const struct pios_ppm_cfg pios_ppm_pin8_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[5], - .num_channels = 1, -}; - -#endif /* PIOS_INCLUDE_PPM */ - -#if defined(PIOS_INCLUDE_PPM_FLEXI) -#include - -const struct pios_ppm_cfg pios_ppm_flexi_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_ppm_flexi_port, - .num_channels = 1, -}; - -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - -/* - * PWM Inputs - */ -#if defined(PIOS_INCLUDE_PWM) -#include - -const struct pios_pwm_cfg pios_pwm_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_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), -}; - -const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Leave the first channel for PPM use and use the rest for PWM */ - .channels = &pios_tim_rcvrport_all_channels[1], - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, -}; - -#endif /* if defined(PIOS_INCLUDE_PWM) */ - - -/* - * 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_HCSR04) */ - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ - -void PIOS_I2C_flexi_adapter_ev_irq_handler(void); -void PIOS_I2C_flexi_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_flexi_adapter_id; -void PIOS_I2C_flexi_adapter_ev_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); -} - -void PIOS_I2C_flexi_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); -} - -#endif /* PIOS_INCLUDE_I2C */ - -#if defined(PIOS_INCLUDE_GCSRCVR) -#include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#if defined(PIOS_INCLUDE_RCVR) -#include "pios_rcvr_priv.h" - -#endif /* PIOS_INCLUDE_RCVR */ - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg_cc = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .vsense_active_low = false -}; - -static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .vsense_active_low = false -}; - -#include "pios_usb_board_data_priv.h" -#include "pios_usb_desc_hid_cdc_priv.h" -#include "pios_usb_desc_hid_only_priv.h" - -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_COM_MSG) - -#include - -#endif /* PIOS_INCLUDE_COM_MSG */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_RCTX) -#include - -const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { - .data_if = 2, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_RCTX */ - -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/Makefile b/flight/targets/boards/coptercontrol/bootloader/Makefile deleted file mode 100644 index 7c575411b..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/Makefile +++ /dev/null @@ -1,32 +0,0 @@ -# -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# -# 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 OPENPILOT_IS_COOL - $(error Top level Makefile must be used to build this target) -endif - -## The standard CMSIS startup -SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c - -include ../board-info.mk -include $(ROOT_DIR)/make/firmware-defs.mk -include $(ROOT_DIR)/make/boot-defs.mk -include $(ROOT_DIR)/make/common-defs.mk - -$(info Making bootloader for CopterControl, board revision $(BOARD_REVISION)) -$(info Use BOARD_REVISION=1 for CC, BOARD_REVISION=2 for CC3D (default)) diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/common.h b/flight/targets/boards/coptercontrol/bootloader/inc/common.h deleted file mode 100644 index e64b89014..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ -#ifndef COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h deleted file mode 100644 index 43182e435..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * @{ - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. - * Central compile time config for the project. - * In particular, pios_config.h is where you define which PiOS libraries - * and features are included in the firmware. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_CONFIG_H -#define PIOS_CONFIG_H - -/* Enable/Disable PiOS modules */ -#define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_IRQ -#define PIOS_INCLUDE_GPIO -#define PIOS_INCLUDE_USB -#define PIOS_INCLUDE_USB_HID -#define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_IAP -#define PIOS_INCLUDE_COM -#define PIOS_INCLUDE_COM_MSG -#define PIOS_INCLUDE_BL_HELPER -#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT - -#endif /* PIOS_CONFIG_H */ - -/** - * @} - * @} - */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h deleted file mode 100644 index 5a40bf4cf..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_USB_BOARD Board specific USB definitions - * @brief Board specific USB definitions - * @{ - * - * @file pios_usb_board_data.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Board specific USB definitions - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_USB_BOARD_DATA_H -#define PIOS_USB_BOARD_DATA_H - -// Note : changing below length will require changes to the USB buffer setup -#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 - -#define PIOS_USB_BOARD_EP_NUM 2 - -#include /* struct usb_* */ - -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" - -/* - * The bootloader uses a simplified report structure - * BL: ... - * FW: ... - * This define changes the behaviour in pios_usb_hid.c - */ -#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - -#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c deleted file mode 100644 index 15068627c..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/main.c +++ /dev/null @@ -1,223 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file main.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This is the file with the main function of the OpenPilot BootLoader - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include -#include -#include -#include -#include -#include -#include - -/* Prototype of PIOS_Board_Init() function */ -extern void PIOS_Board_Init(void); -extern void FLASH_Download(); -#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1) - -/* Private typedef -----------------------------------------------------------*/ -typedef void (*pFunction)(void); -/* Private define ------------------------------------------------------------*/ -/* Private macro -------------------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ -pFunction Jump_To_Application; -uint32_t JumpAddress; - -/// LEDs PWM -uint32_t period1 = 5000; // 5 mS -uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS -uint32_t period2 = 5000; // 5 mS -uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS - - -//////////////////////////////////////// -uint8_t tempcount = 0; - -/* Extern variables ----------------------------------------------------------*/ -DFUStates DeviceState; -int16_t status = 0; -uint8_t JumpToApp = FALSE; -uint8_t GO_dfu = FALSE; -uint8_t USB_connected = FALSE; -uint8_t User_DFU_request = FALSE; -static uint8_t mReceive_Buffer[63]; -/* Private function prototypes -----------------------------------------------*/ -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); -uint8_t processRX(); -void jump_to_app(); - -int main() -{ - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); - - USB_connected = PIOS_USB_CableConnected(0); - - if (PIOS_IAP_CheckRequest() == TRUE) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = TRUE; - PIOS_IAP_ClearRequest(); - } - - GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); - - if (GO_dfu == TRUE) { - PIOS_Board_Init(); - if (User_DFU_request == TRUE) { - DeviceState = DFUidle; - } else { - DeviceState = BLidle; - } - } else { - JumpToApp = TRUE; - } - - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (TRUE) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; - - if (JumpToApp == TRUE) { - jump_to_app(); - } - - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default: // error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } - - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) { - PIOS_LED_On(PIOS_LED_HEARTBEAT); - } else { - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } - } else { - PIOS_LED_On(PIOS_LED_HEARTBEAT); - } - - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) { - PIOS_LED_On(PIOS_LED_HEARTBEAT); - } else { - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } - } else { - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } - - if (stopwatch > 50 * 1000 * 1000) { - stopwatch = 0; - } - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { - JumpToApp = TRUE; - } - - processRX(); - DataDownload(start); - } -} - -void jump_to_app() -{ - const struct pios_board_info *bdinfo = &pios_board_info_blob; - - if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - _SetCNTR(0); // clear interrupt mask - _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); - Jump_To_Application = (pFunction)JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } -} -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) -{ - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; -} - -uint8_t processRX() -{ - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return TRUE; -} - -int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) -{ - return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); -} diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c deleted file mode 100644 index e0a63193a..000000000 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ /dev/null @@ -1,106 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board specific static initializers for hardware for the CopterControl board. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include -#include - -/* - * Pull in the board-specific static HW definitions. - * Including .c files is a bit ugly but this allows all of - * the HW definitions to be const and static to limit their - * scope. - * - * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE - */ -#include "../board_hw_defs.c" - -uint32_t pios_com_telem_usb_id; - -/** - * PIOS_Board_Init() - * initializes all the core subsystems on this specific hardware - * called from System/openpilot.c - */ -static bool board_init_complete = false; -void PIOS_Board_Init(void) -{ - if (board_init_complete) { - return; - } - - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); - - /* Delay system */ - PIOS_DELAY_Init(); - - const struct pios_board_info *bdinfo = &pios_board_info_blob; - -#if defined(PIOS_INCLUDE_LED) - const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ - -#if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); - - uint32_t pios_usb_id; - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - -#endif /* PIOS_INCLUDE_USB */ - - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar - - board_init_complete = true; -} - -void PIOS_ADC_DMA_Handler() -{} diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile deleted file mode 100644 index d58b57e64..000000000 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ /dev/null @@ -1,148 +0,0 @@ -# -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot -# -# 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 OPENPILOT_IS_COOL - $(error Top level Makefile must be used to build this target) -endif - -include ../board-info.mk -include $(ROOT_DIR)/make/firmware-defs.mk - -# ARM DSP library -USE_DSP_LIB ?= NO - -# Set to YES to build a FW version that will erase data flash memory -ERASE_FLASH ?= NO - -# Set to yes to include Gcsreceiver module -GCSRECEIVER ?= NO - -# Enable Diag tasks ? -DIAG_TASKS ?= NO - -# List of mandatory modules to include -MODULES += Attitude -MODULES += Stabilization -MODULES += Actuator -MODULES += Receiver -MODULES += ManualControl -MODULES += FirmwareIAP -MODULES += Telemetry - -# List of optional modules to include -OPTMODULES += CameraStab -OPTMODULES += ComUsbBridge -OPTMODULES += GPS -OPTMODULES += TxPID -OPTMODULES += Osd/osdoutput -#OPTMODULES += Altitude -#OPTMODULES += Fault - -SRC += $(FLIGHTLIB)/notification.c - -# Include all camera options -CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF - - - -# Erase flash firmware should be buildable from command line -ifeq ($(ERASE_FLASH), YES) - CDEFS += -DERASE_FLASH -endif - -# List C source files here (C dependencies are automatically generated). -# Use file-extension c for "c-only"-files -ifndef TESTAPP - ## The standard CMSIS startup - SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c - - ## Application Core - SRC += ../pios_usb_board_data.c - SRC += $(OPMODULEDIR)/System/systemmod.c - SRC += $(OPSYSTEM)/coptercontrol.c - SRC += $(OPSYSTEM)/pios_board.c - SRC += $(FLIGHTLIB)/alarms.c - SRC += $(FLIGHTLIB)/instrumentation.c - SRC += $(OPUAVTALK)/uavtalk.c - SRC += $(OPUAVOBJ)/uavobjectmanager.c - SRC += $(OPUAVOBJ)/uavobjectpersistence.c - SRC += $(OPUAVOBJ)/eventdispatcher.c - - ## UAVObjects - SRC += $(OPUAVSYNTHDIR)/accessorydesired.c - SRC += $(OPUAVSYNTHDIR)/objectpersistence.c - SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c - SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c - SRC += $(OPUAVSYNTHDIR)/faultsettings.c - SRC += $(OPUAVSYNTHDIR)/flightstatus.c - SRC += $(OPUAVSYNTHDIR)/systemstats.c - SRC += $(OPUAVSYNTHDIR)/systemalarms.c - SRC += $(OPUAVSYNTHDIR)/systemsettings.c - SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c - SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c - SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c - SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c - SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c - SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c - SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c - SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c - SRC += $(OPUAVSYNTHDIR)/actuatordesired.c - SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c - SRC += $(OPUAVSYNTHDIR)/accelstate.c - SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c - SRC += $(OPUAVSYNTHDIR)/gyrostate.c - SRC += $(OPUAVSYNTHDIR)/attitudestate.c - SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c - SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c - SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c - SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c - SRC += $(OPUAVSYNTHDIR)/mixersettings.c - SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c - SRC += $(OPUAVSYNTHDIR)/attitudesettings.c - SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c - SRC += $(OPUAVSYNTHDIR)/cameradesired.c - SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c - SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c - SRC += $(OPUAVSYNTHDIR)/gpssettings.c - SRC += $(OPUAVSYNTHDIR)/hwsettings.c - SRC += $(OPUAVSYNTHDIR)/receiveractivity.c - SRC += $(OPUAVSYNTHDIR)/mixerstatus.c - SRC += $(OPUAVSYNTHDIR)/ratedesired.c - SRC += $(OPUAVSYNTHDIR)/txpidsettings.c - SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c - # Command line option for Gcsreceiver module - ifeq ($(GCSRECEIVER), YES) - SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c - endif - # Enable Diag tasks and UAVOs needed - ifeq ($(DIAG_TASKS), YES) - CDEFS += -DDIAG_TASKS - SRC += $(OPUAVSYNTHDIR)/taskinfo.c - SRC += $(OPUAVSYNTHDIR)/callbackinfo.c - SRC += $(OPUAVSYNTHDIR)/perfcounter.c - SRC += $(OPUAVSYNTHDIR)/i2cstats.c - endif -else - ## Test Code - SRC += $(OPTESTS)/test_common.c - SRC += $(OPTESTS)/$(TESTAPP).c -endif - -include $(ROOT_DIR)/make/apps-defs.mk -include $(ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c deleted file mode 100644 index e748f6317..000000000 --- a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c +++ /dev/null @@ -1,117 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @brief These files are the core system files of OpenPilot. - * They are the ground layer just above PiOS. In practice, OpenPilot actually starts - * in the main() function of openpilot.c - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @brief This is where the OP firmware starts. Those files also define the compile-time - * options of the firmware. - * @{ - * @file openpilot.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Sets up and runs main OpenPilot tasks. - * @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 "inc/openpilot.h" -#include -#include - -/* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) - -/* Global Variables */ - -/* Prototype of PIOS_Board_Init() function */ -extern void PIOS_Board_Init(void); -extern void Stack_Change(void); - -/** - * OpenPilot Main function: - * - * Initialize PiOS
- * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
- * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) - * If something goes wrong, blink LED1 and LED2 every 100ms - * - */ -int main() -{ - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); - -#ifdef ERASE_FLASH - PIOS_Flash_Jedec_EraseChip(); -#if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - while (1) { - ; - } -#endif - - /* Initialize modules */ - MODULE_INITIALISE_ALL - /* swap the stack to use the IRQ stack */ - Stack_Change(); - - /* Start the FreeRTOS scheduler, which should never return. - * - * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls - * (schedules) function files (modules). These functions never return from their - * while loops, which explains why each module has a while(1){} segment. Thus, - * the OpenPilot software actually starts at the vTaskStartScheduler() function, - * even though this is somewhat obscure. - * - * In addition, there are many main() functions in the OpenPilot firmware source tree - * This is because each main() refers to a separate hardware platform. Of course, - * C only allows one main(), so only the relevant main() function is compiled when - * making a specific firmware. - * - */ - vTaskStartScheduler(); - - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - while (1) { -#if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - PIOS_DELAY_WaitmS(100); - } - - return 0; -} - -/** - * @} - * @} - */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h deleted file mode 100644 index 429d86ade..000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef FREERTOS_CONFIG_H -#define FREERTOS_CONFIG_H - -/*----------------------------------------------------------- -* Application specific definitions. -* -* These definitions should be adjusted for your particular hardware and -* application requirements. -* -* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE -* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. -* -* See http://www.freertos.org/a00110.html. -*----------------------------------------------------------*/ - -/** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ - -/* Notes: We use 5 task priorities */ - -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_MALLOC_FAILED_HOOK 1 -#define configCPU_CLOCK_HZ ((unsigned long)72000000) -#define configTICK_RATE_HZ ((portTickType)1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)48) -#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) -#define configMAX_TASK_NAME_LEN (6) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configQUEUE_REGISTRY_SIZE 0 - -/* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES (2) - -/* Set the following definitions to 1 to include the API function, or zero - to exclude the API function. */ - -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 0 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 - -/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 - (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ - #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ - -/* This is the value being used as per the ST library which permits 16 - priority values, 0 to 15. This must correspond to the - configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest - NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 - -#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) -#define CHECK_IRQ_STACK -#endif - -/* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 -#define INCLUDE_xTaskGetIdleTaskHandle 1 -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } \ - while (0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ - -#ifdef DIAG_TASKS -#define configCHECK_FOR_STACK_OVERFLOW 2 -#else -#define configCHECK_FOR_STACK_OVERFLOW 1 -#endif - -/** - * @} - */ - -#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h deleted file mode 100644 index 16364d7fa..000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h +++ /dev/null @@ -1,81 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#ifndef PIOS_BOARD_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -// #define PIOS_LED_LED1_GPIO_PORT GPIOC -// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -// #define PIOS_LED_LED2_GPIO_PORT GPIOC -// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } - - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX -#endif - -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h deleted file mode 100644 index a371b908f..000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ /dev/null @@ -1,188 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @{ - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. - * @brief PiOS configuration header, the compile time config file for the PIOS. - * Defines which PiOS libraries and features are included in the firmware. - * @see The GNU Public License (GPL) Version 3 - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_CONFIG_H -#define PIOS_CONFIG_H - -/* - * Below is a complete list of PIOS configurable options. - * Please do not remove or rearrange them. Only comment out - * unused options in the list. See main pios.h header for more - * details. - */ - -/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ -/* #define DEBUG_LEVEL 0 */ -/* #define PIOS_ENABLE_DEBUG_PINS */ - -/* PIOS FreeRTOS support */ -#define PIOS_INCLUDE_FREERTOS - - -/* PIOS CallbackScheduler support */ -#define PIOS_INCLUDE_CALLBACKSCHEDULER - -/* PIOS bootloader helper */ -#define PIOS_INCLUDE_BL_HELPER -/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ - -/* PIOS system functions */ -#define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_INITCALL -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_TASK_MONITOR -// #define PIOS_INCLUDE_INSTRUMENTATION -#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 - -/* PIOS hardware peripherals */ -#define PIOS_INCLUDE_IRQ -#define PIOS_INCLUDE_RTC -#define PIOS_INCLUDE_TIM -#define PIOS_INCLUDE_USART -#define PIOS_INCLUDE_ADC -/* #define PIOS_INCLUDE_I2C */ -#define PIOS_INCLUDE_SPI -#define PIOS_INCLUDE_GPIO -#define PIOS_INCLUDE_EXTI -#define PIOS_INCLUDE_WDG - -/* PIOS USB functions */ -#define PIOS_INCLUDE_USB -#define PIOS_INCLUDE_USB_HID -#define PIOS_INCLUDE_USB_CDC -#define PIOS_INCLUDE_USB_RCTX - -/* PIOS sensor interfaces */ -#define PIOS_INCLUDE_ADXL345 -/* #define PIOS_INCLUDE_BMA180 */ -/* #define PIOS_INCLUDE_L3GD20 */ -#define PIOS_INCLUDE_MPU6000 -#define PIOS_MPU6000_ACCEL -/* #define PIOS_INCLUDE_HMC5843 */ -/* #define PIOS_INCLUDE_HMC5X83 */ -/* #define PIOS_HMC5883_HAS_GPIOS */ -/* #define PIOS_INCLUDE_BMP085 */ -/* #define PIOS_INCLUDE_MS5611 */ -/* #define PIOS_INCLUDE_MPXV */ -/* #define PIOS_INCLUDE_ETASV3 */ -/* #define PIOS_INCLUDE_HCSR04 */ - -#define PIOS_SENSOR_RATE 500.0f - -/* PIOS receiver drivers */ -#define PIOS_INCLUDE_PWM -#define PIOS_INCLUDE_PPM -#define PIOS_INCLUDE_PPM_FLEXI -#define PIOS_INCLUDE_DSM -#define PIOS_INCLUDE_SBUS -/* #define PIOS_INCLUDE_GCSRCVR */ -/* #define PIOS_INCLUDE_OPLINKRCVR */ - -/* PIOS abstract receiver interface */ -#define PIOS_INCLUDE_RCVR - -/* PIOS common peripherals */ -#define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_IAP -#define PIOS_INCLUDE_SERVO -/* #define PIOS_INCLUDE_I2C_ESC */ -/* #define PIOS_INCLUDE_OVERO */ -/* #define PIOS_OVERO_SPI */ -/* #define PIOS_INCLUDE_SDCARD */ -/* #define LOG_FILENAME "startup.log" */ -#define PIOS_INCLUDE_FLASH -#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS -/* #define FLASH_FREERTOS */ -/* #define PIOS_INCLUDE_FLASH_EEPROM */ -/* #define PIOS_INCLUDE_FLASH_INTERNAL */ - -/* PIOS radio modules */ -/* #define PIOS_INCLUDE_RFM22B */ -/* #define PIOS_INCLUDE_RFM22B_COM */ -/* #define PIOS_INCLUDE_PPM_OUT */ -/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ - -/* PIOS misc peripherals */ -/* #define PIOS_INCLUDE_VIDEO */ -/* #define PIOS_INCLUDE_WAVE */ -/* #define PIOS_INCLUDE_UDP */ - -/* PIOS abstract comms interface with options */ -#define PIOS_INCLUDE_COM -/* #define PIOS_INCLUDE_COM_MSG */ -#define PIOS_INCLUDE_TELEMETRY_RF -/* #define PIOS_INCLUDE_COM_TELEM */ -/* #define PIOS_INCLUDE_COM_FLEXI */ -/* #define PIOS_INCLUDE_COM_AUX */ -/* #define PIOS_TELEM_PRIORITY_QUEUE */ -#define PIOS_INCLUDE_GPS -#define PIOS_GPS_MINIMAL -/* #define PIOS_INCLUDE_GPS_NMEA_PARSER */ -#define PIOS_INCLUDE_GPS_UBX_PARSER -/* #define PIOS_GPS_SETS_HOMELOCATION */ - -/* Stabilization options */ -/* #define PIOS_QUATERNION_STABILIZATION */ -#define PIOS_EXCLUDE_ADVANCED_FEATURES -/* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 - -/* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 - -/* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 820 -#define PIOS_MANUAL_STACK_SIZE 735 -#define PIOS_RECEIVER_STACK_SIZE 620 -#define PIOS_STABILIZATION_STACK_SIZE 400 - -#ifdef DIAG_TASKS -#define PIOS_SYSTEM_STACK_SIZE 740 -#else -#define PIOS_SYSTEM_STACK_SIZE 660 -#endif -#define PIOS_TELEM_RX_STACK_SIZE 410 -#define PIOS_TELEM_TX_STACK_SIZE 560 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 95 - -/* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 - -/* Revolution series */ -/* #define REVOLUTION */ - -#endif /* PIOS_CONFIG_H */ -/** - * @} - * @} - */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h deleted file mode 100644 index 4e8aa47c6..000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. - * Central compile time config for the project. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#ifndef PIOS_CONFIG_POSIX_H -#define PIOS_CONFIG_POSIX_H - - -/* Enable/Disable PiOS Modules */ -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_FREERTOS -#define PIOS_INCLUDE_CALLBACKSCHEDULER -#define PIOS_INCLUDE_TASK_MONITOR -#define PIOS_INCLUDE_COM -#define PIOS_INCLUDE_UDP -#define PIOS_INCLUDE_SERVO - - -/* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 - -#endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h deleted file mode 100644 index 1fac7c659..000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h +++ /dev/null @@ -1,47 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_USB_BOARD Board specific USB definitions - * @brief Board specific USB definitions - * @{ - * - * @file pios_usb_board_data.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Board specific USB definitions - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_USB_BOARD_DATA_H -#define PIOS_USB_BOARD_DATA_H - -// Note : changing below length will require changes to the USB buffer setup -#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 -#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 -#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 - -#define PIOS_USB_BOARD_EP_NUM 4 - -#include /* USB_* macros */ - -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" - -#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c deleted file mode 100644 index 474a21330..000000000 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ /dev/null @@ -1,920 +0,0 @@ -/** - ***************************************************************************** - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @{ - * @brief Defines board specific static initializers for hardware for the CopterControl board. - *****************************************************************************/ -/* - * 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 "inc/openpilot.h" -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef PIOS_INCLUDE_INSTRUMENTATION -#include -#endif -#if defined(PIOS_INCLUDE_ADXL345) -#include -#endif - -/* - * Pull in the board-specific static HW definitions. - * Including .c files is a bit ugly but this allows all of - * the HW definitions to be const and static to limit their - * scope. - * - * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE - */ -#include "../board_hw_defs.c" - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook(); -static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 - -#define PIOS_COM_GPS_RX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#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; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_telem_rf_id; -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; - -uintptr_t pios_uavo_settings_fs_id; -uintptr_t pios_user_fs_id = 0; -/** - * Configuration for MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line3, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 8 for 1000 Hz - .Smpl_rate_div_no_dlp = 7, - // Clock at 1 khz, downsampled by 1 for 1000 Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 2 -}; -#endif /* PIOS_INCLUDE_MPU6000 */ - -/** - * PIOS_Board_Init() - * initializes all the core subsystems on this specific hardware - * called from System/openpilot.c - */ -int32_t init_test; -void PIOS_Board_Init(void) -{ - /* Delay system */ - PIOS_DELAY_Init(); - - const struct pios_board_info *bdinfo = &pios_board_info_blob; - -#if defined(PIOS_INCLUDE_LED) - const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ - -#ifdef PIOS_INCLUDE_INSTRUMENTATION - PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); -#endif - -#if defined(PIOS_INCLUDE_SPI) - /* Set up the SPI interface to the serial flash */ - - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { - PIOS_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { - PIOS_Assert(0); - } - break; - default: - PIOS_Assert(0); - } - -#endif - - uintptr_t flash_id; - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - } - - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - PIOS_CALLBACKSCHEDULER_Initialize(); - - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - -#if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); -#endif - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { - PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); - PIOS_IAP_WriteBootCmd(0, 0); - PIOS_IAP_WriteBootCmd(1, 0); - PIOS_IAP_WriteBootCmd(2, 0); - } - - HwSettingsInitialize(); - -#ifndef ERASE_FLASH -#ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); -#endif -#endif - - /* Initialize the alarms library */ - AlarmsInitialize(); - - /* Check for repeated boot failures */ - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } - - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); - -#if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; -#endif - - uint32_t pios_usb_id; - - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: -#if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - -#endif /* PIOS_INCLUDE_USB */ - - /* Configure the main IO port */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - uint8_t hwsettings_cc_mainport; - HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); - - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DISABLED: - break; - case HWSETTINGS_CC_MAINPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case HWSETTINGS_CC_MAINPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_MAINPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_main_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - 0)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_MAINPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - 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 *)pios_malloc(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 */ - uint8_t hwsettings_cc_flexiport; - HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); - - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - case HWSETTINGS_CC_FLEXIPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: -#if defined(PIOS_INCLUDE_PPM_FLEXI) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; - case HWSETTINGS_CC_FLEXIPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_flexi_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - hwsettings_DSMxBind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_FLEXIPORT_I2C: -#if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { - PIOS_Assert(0); - } - } -#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 *)pios_malloc(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 */ - uint8_t hwsettings_rcvrport; - HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); - - switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: -#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_PWMNOONESHOT: -#if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: -#if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg); - } else { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - } - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: - /* This is a combination of PPM and PWM inputs */ -#if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ -#if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: - break; - } - -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - - /* Remap AFIO pin for PB4 (Servo 5 Out)*/ - GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); - -#ifndef PIOS_ENABLE_DEBUG_PINS - switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: - case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: - PIOS_Servo_Init(&pios_servo_cfg); - break; - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: - PIOS_Servo_Init(&pios_servo_rcvr_cfg); - break; - } -#else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); -#endif /* PIOS_ENABLE_DEBUG_PINS */ - - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - // Revision 1 with invensense gyros, start the ADC -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif -#if defined(PIOS_INCLUDE_ADXL345) - PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); -#endif - break; - case BOARD_REVISION_CC3D: - // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it - GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); - -#if defined(PIOS_INCLUDE_MPU6000) - // Set up the SPI interface to the serial flash - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_Assert(0); - } - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - init_test = !PIOS_MPU6000_Driver.test(0); -#endif /* PIOS_INCLUDE_MPU6000 */ - - break; - default: - PIOS_Assert(0); - } - - /* Make sure we have at least one telemetry link configured or else fail initialization */ - PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); - - // Attach the board config check hook - SANITYCHECK_AttachHook(&CopterControlConfigHook); - // trigger a config check if actuatorsettings are updated - ActuatorSettingsInitialize(); - ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); -} - -SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook() -{ - // inhibit usage of oneshot for non supported RECEIVER port modes - uint8_t recmode; - - HwSettingsCC_RcvrPortGet(&recmode); - uint8_t flexiMode; - uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM]; - ActuatorSettingsBankModeGet(modes); - HwSettingsCC_FlexiPortGet(&flexiMode); - - switch ((HwSettingsCC_RcvrPortOptions)recmode) { - // Those modes allows oneshot usage - case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: - case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: - if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT || - flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) && - (modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || - modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125)) { - return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT; - } else { - return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; - } - - // inhibit oneshot for the following modes - case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: - case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: - for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { - if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || - modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) { - return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; - } - - return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; - } - } - return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; -} -// trigger a configuration check if ActuatorSettings are changed. -void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - configuration_check(); -} - -/** - * @} - */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c deleted file mode 100644 index 7fa958a38..000000000 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ /dev/null @@ -1,145 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board specific static initializers for hardware for the OpenPilot board. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "inc/openpilot.h" -#include -#include -#include - -/** - * PIOS_Board_Init() - * initializes all the core systems on this specific hardware - * called from System/openpilot.c - */ -void PIOS_Board_Init(void) -{ - /* Delay system */ - PIOS_DELAY_Init(); - - /* Initialize the delayed callback library */ - PIOS_CALLBACKSCHEDULER_Initialize(); - - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - /* Initialize the alarms library */ - AlarmsInitialize(); - - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the PiOS library */ - PIOS_COM_Init(); -} - - -const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, -}; -const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, -}; -const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, -}; - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, -}; -#endif - -/* - * Board specific number of devices. - */ -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, -#endif -}; - -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); - -/* - * COM devices - */ - -/* - * Board specific number of devices. - */ -extern const struct pios_com_driver pios_serial_com_driver; -extern const struct pios_com_driver pios_udp_com_driver; - -struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, -#ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, -#endif -}; - -const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); - -/** - * @} - */ diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h deleted file mode 100644 index f3c3e34d3..000000000 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ /dev/null @@ -1,279 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_BOARD_H -#define PIOS_BOARD_H -// ------------------------ -// 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 - ------+-----------+-----------+-----------+---------- - */ - -// ------------------------ -// DMA Channels Used -// ------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ - -// ------------------------ -// BOOTLOADER_SETTINGS -// ------------------------ -#define BOARD_READABLE TRUE -#define BOARD_WRITABLE TRUE -#define MAX_DEL_RETRYS 3 - -// ------------------------ -// WATCHDOG_SETTINGS -// ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_AUTOTUNE 0x0010 - -// ------------------------ -// TELEMETRY -// ------------------------ -#define TELEM_QUEUE_SIZE 10 - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_HEARTBEAT 0 - -// ------------------------- -// System Settings -// ------------------------- -#define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) - -// ------------------------- -// Interrupt Priorities -// ------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -// ------------------------ -// PIOS_I2C -// See also pios_board.c -// ------------------------ -#define PIOS_I2C_MAX_DEVS 1 -extern uint32_t pios_i2c_flexi_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) - -// ------------------------ -// PIOS_BMP085 -// ------------------------ -#define PIOS_BMP085_OVERSAMPLING 3 - -// ------------------------- -// SPI -// -// See also pios_board.c -// ------------------------- -#define PIOS_SPI_MAX_DEVS 2 - -// ------------------------- -// PIOS_USART -// ------------------------- -#define PIOS_USART_MAX_DEVS 2 - -// ------------------------- -// PIOS_COM -// -// See also pios_board.c -// ------------------------- -#define PIOS_COM_MAX_DEVS 3 - -extern uint32_t pios_com_telem_rf_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) - -#if defined(PIOS_INCLUDE_GPS) -extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#endif /* PIOS_INCLUDE_GPS */ - -extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) - -extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) - -extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -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 -// PIOS_ADC_PinGet(1) = Gyro Y -// PIOS_ADC_PinGet(2) = Gyro X -// ------------------------- -// #define PIOS_ADC_OVERSAMPLING_RATE 1 -#define PIOS_ADC_USE_TEMP_SENSOR 1 -#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 -#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 - -#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) -#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 -#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 -#define PIOS_ADC_PIN1_ADC ADC2 -#define PIOS_ADC_PIN1_ADC_NUMBER 1 - -#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) -#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 -#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 -#define PIOS_ADC_PIN2_ADC ADC1 -#define PIOS_ADC_PIN2_ADC_NUMBER 2 - -#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) -#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 -#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 -#define PIOS_ADC_PIN3_ADC ADC2 -#define PIOS_ADC_PIN3_ADC_NUMBER 2 - -#define PIOS_ADC_NUM_PINS 3 - -#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } -#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } -#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } -#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } -#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } -#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) -#define PIOS_ADC_NUM_ADC_CHANNELS 2 -#define PIOS_ADC_USE_ADC2 1 -#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) -#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 -/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ -/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ -/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ -/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ -#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 -/* Sample time: */ -/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ -/* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ -/* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW - -// Currently analog acquistion hard coded at 480 Hz -// PCKL2 = HCLK / 16 -// ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define PIOS_ADC_MAX_OVERSAMPLING 48 - -#define PIOS_ADC_TEMPERATURE_PIN 0 - -// ------------------------ -// PIOS_RCVR -// See also pios_board.c -// ------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_GCSRCVR_TIMEOUT_MS 100 - -// ------------------------- -// Receiver PPM input -// ------------------------- -#define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 12 - -// ------------------------- -// Receiver PWM input -// ------------------------- -#define PIOS_PWM_MAX_DEVS 1 -#define PIOS_PWM_NUM_INPUTS 6 - -// ------------------------- -// Receiver DSM input -// ------------------------- -#define PIOS_DSM_MAX_DEVS 2 -#define PIOS_DSM_NUM_INPUTS 12 - -// ------------------------- -// Receiver S.Bus input -// ------------------------- -#define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16 + 2) - -// ------------------------- -// Servo outputs -// ------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -#define PIOS_SERVO_BANKS 6 - -// -------------------------- -// Timer controller settings -// -------------------------- -#define PIOS_TIM_MAX_DEVS 3 - -// ------------------------- -// GPIO -// ------------------------- -#define PIOS_GPIO_PORTS {} -#define PIOS_GPIO_PINS {} -#define PIOS_GPIO_CLKS {} -#define PIOS_GPIO_NUM 0 - -// ------------------------- -// USB -// ------------------------- -#define PIOS_USB_HID_MAX_DEVS 1 - -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIOC -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/pios_usb_board_data.c b/flight/targets/boards/coptercontrol/pios_usb_board_data.c deleted file mode 100644 index 20c49fdcf..000000000 --- a/flight/targets/boards/coptercontrol/pios_usb_board_data.c +++ /dev/null @@ -1,102 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_USB_BOARD Board specific USB definitions - * @brief Board specific USB definitions - * @{ - * - * @file pios_usb_board_data.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Board specific USB definitions - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ - -static const uint8_t usb_product_id[28] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'C', 0, - 'o', 0, - 'p', 0, - 't', 0, - 'e', 0, - 'r', 0, - 'C', 0, - 'o', 0, - 'n', 0, - 't', 0, - 'r', 0, - 'o', 0, - 'l', 0, -}; - -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, -}; - -static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), -}; - -static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 -}; - -int32_t PIOS_USB_BOARD_DATA_Init(void) -{ - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - - PIOS_SYS_SerialNumberGet((char *)sn); - - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t *utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); - - return 0; -} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index f0d0e483f..4d9c3064c 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -24,6 +24,9 @@ endif include ../board-info.mk include $(ROOT_DIR)/make/firmware-defs.mk +# REVO C++ support +USE_CXX = YES + # ARM DSP library USE_DSP_LIB ?= NO @@ -70,7 +73,7 @@ ifndef TESTAPP ## Application Core SRC += ../pios_usb_board_data.c SRC += $(OPMODULEDIR)/System/systemmod.c - SRC += $(OPSYSTEM)/discoveryf4bare.c + CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp SRC += $(OPSYSTEM)/pios_board.c SRC += $(FLIGHTLIB)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c @@ -91,6 +94,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/auxmagsupport.c SRC += $(FLIGHTLIB)/lednotification.c + CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index b2fe81720..c51c98393 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -19,6 +19,8 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += statusvtolautotakeoff UAVOBJSRCFILENAMES += vtolselftuningstats @@ -106,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings diff --git a/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c b/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp similarity index 99% rename from flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c rename to flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp index 19e33890b..268325b26 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c +++ b/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp @@ -31,7 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - +extern "C" { #include "inc/openpilot.h" #include @@ -74,6 +74,7 @@ static void initTask(void *parameters); /* Prototype of generated InitModules() function */ extern void InitModules(void); +} /** * OpenPilot Main function: diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 4d52296cc..1b169634b 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1159,6 +1159,17 @@ static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { }, }, + .dtr = { + // FlexIO pin 9 + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + }, + }, + .tx = { // * 7: PC6 = TIM8 CH1, USART6 TX .gpio = GPIOC, diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index d411a525e..587bf0601 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 9e77c42a5..5bfc0a16c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -887,6 +887,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; + case HWSETTINGS_RM_RCVRPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; } #if defined(PIOS_INCLUDE_GCSRCVR) diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index c040a0421..a47856179 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -24,6 +24,9 @@ endif include ../board-info.mk include $(ROOT_DIR)/make/firmware-defs.mk +# REVO C++ support +USE_CXX = YES + # ARM DSP library USE_DSP_LIB ?= NO @@ -65,7 +68,7 @@ ifndef TESTAPP ## Application Core SRC += ../pios_usb_board_data.c SRC += $(OPMODULEDIR)/System/systemmod.c - SRC += $(OPSYSTEM)/revolution.c + CPPSRC += $(OPSYSTEM)/revolution.cpp SRC += $(OPSYSTEM)/pios_board.c SRC += $(FLIGHTLIB)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c @@ -84,6 +87,8 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/auxmagsupport.c + CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp + ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 377fea983..e0931a700 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings diff --git a/flight/targets/boards/revoproto/firmware/revolution.c b/flight/targets/boards/revoproto/firmware/revolution.cpp similarity index 99% rename from flight/targets/boards/revoproto/firmware/revolution.c rename to flight/targets/boards/revoproto/firmware/revolution.cpp index b1081c4b9..268325b26 100644 --- a/flight/targets/boards/revoproto/firmware/revolution.c +++ b/flight/targets/boards/revoproto/firmware/revolution.cpp @@ -31,6 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +extern "C" { #include "inc/openpilot.h" #include @@ -73,6 +74,7 @@ static void initTask(void *parameters); /* Prototype of generated InitModules() function */ extern void InitModules(void); +} /** * OpenPilot Main function: diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 70673a563..7d1f30ff4 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -63,6 +63,8 @@ FLIGHTLIB = ../../../../libraries FLIGHTLIBINC = $(FLIGHTLIB)/inc MATHLIB = $(FLIGHTLIB)/math MATHLIBINC = $(FLIGHTLIB)/math +PIDLIB = $(FLIGHTLIB)/pid +PIDLIBINC = $(FLIGHTLIB)/pid PIOSPOSIX = $(PIOS)/posix PIOSCOMMON = $(PIOS)/posix PIOSCORECOMMON = $(PIOS)/common @@ -111,6 +113,7 @@ SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c SRC += $(MATHLIB)/butterworth.c +CPPSRC += $(PIDLIB)/pidcontroldown.cpp SRC += $(PIOSCORECOMMON)/pios_task_monitor.c ifeq ($(USE_YAFFS),YES) @@ -145,6 +148,7 @@ EXTRAINCDIRS += $(OPUAVOBJINC) EXTRAINCDIRS += $(UAVOBJSYNTHDIR) EXTRAINCDIRS += $(FLIGHTLIBINC) EXTRAINCDIRS += $(MATHLIBINC) +EXTRAINCDIRS += $(PIDLIBINC) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(CMSISDIR) EXTRAINCDIRS += $(OPUAVSYNTHDIR) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro index dee478c28..1f27742bd 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro @@ -2,3 +2,5 @@ TEMPLATE = subdirs SUBDIRS = src \ + +QT += xml diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp index ca615f94b..ad3033bd0 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp @@ -55,13 +55,6 @@ QVector AllLayersOfType::GetAllLayersOfType(const MapType::Types } break; - case MapType::YahooHybrid: - { - types.append(MapType::YahooSatellite); - types.append(MapType::YahooLabels); - } - break; - case MapType::ArcGIS_MapsLT_Map_Hybrid: { types.append(MapType::ArcGIS_MapsLT_OrtoFoto); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro index d606082a1..912d292fe 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro @@ -38,3 +38,5 @@ HEADERS += opmaps.h \ kibertilecache.h \ debugheader.h \ diagnostics.h + +QT += xml diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/maptype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/maptype.h index d31844f6b..8e5569764 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/maptype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/maptype.h @@ -52,11 +52,6 @@ public: OpenStreetMapSurfer = 34, OpenStreetMapSurferTerrain = 35, - YahooMap = 64, - YahooSatellite = 128, - YahooLabels = 256, - YahooHybrid = 333, - BingMap = 444, BingSatellite = 555, BingHybrid = 666, diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp index fd249f66d..a39cf8b01 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp @@ -153,15 +153,6 @@ QByteArray OPMaps::GetImageFrom(const MapType::Types &type, const Point &pos, co } break; - case MapType::YahooHybrid: - case MapType::YahooLabels: - case MapType::YahooMap: - case MapType::YahooSatellite: - { - qheader.setRawHeader("Referrer", "http://maps.yahoo.com/"); - } - break; - case MapType::ArcGIS_MapsLT_Map_Labels: case MapType::ArcGIS_MapsLT_Map: case MapType::ArcGIS_MapsLT_OrtoFoto: diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp index 50d16bbc9..5e1f1f0c8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp @@ -61,11 +61,6 @@ ProviderStrings::ProviderStrings() /// GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA"; - // Yahoo version strings - VersionYahooMap = "4.3"; - VersionYahooSatellite = "1.9"; - VersionYahooLabels = "4.3"; - // BingMaps VersionBingMaps = "563"; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h index 1d349e7c2..b0d2389d9 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h @@ -60,11 +60,6 @@ public: /// - // Yahoo version strings - QString VersionYahooMap; - QString VersionYahooSatellite; - QString VersionYahooLabels; - // BingMaps QString VersionBingMaps; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp index e033d58dc..a0ebd84d6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp @@ -26,7 +26,8 @@ */ #include "urlfactory.h" #include - +#include +#include namespace core { const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84 @@ -97,6 +98,12 @@ void UrlFactory::TryCorrectGoogleVersions() if (CorrectGoogleVersions && !IsCorrectGoogleVersions()) { QNetworkReply *reply; QNetworkRequest qheader; + // This SSL Hack is half assed... technically bad *security* joojoo. + // Required due to a QT5 bug on linux and Mac + // + QSslConfiguration conf = qheader.sslConfiguration(); + conf.setPeerVerifyMode(QSslSocket::VerifyNone); + qheader.setSslConfiguration(conf); QNetworkAccessManager network; QEventLoop q; QTimer tT; @@ -109,7 +116,10 @@ void UrlFactory::TryCorrectGoogleVersions() qDebug() << "Correct GoogleVersion"; #endif // DEBUG_URLFACTORY // setIsCorrectGoogleVersions(true); - QString url = "https://maps.google.com/maps?output=classic"; + // QString url = "https://www.google.com/maps/@0,-0,7z?dg=dbrw&newdg=1"; + // We need to switch to the Above url... the /lochp method will be depreciated soon + // https://productforums.google.com/forum/#!category-topic/maps/navigation/k6EFrp7J7Jk + QString url = "https://www.google.com/lochp"; qheader.setUrl(QUrl(url)); qheader.setRawHeader("User-Agent", UserAgent); @@ -126,7 +136,12 @@ void UrlFactory::TryCorrectGoogleVersions() #endif // DEBUG_URLFACTORY return; } + QString html = QString(reply->readAll()); +#ifdef DEBUG_URLFACTORY + qDebug() << html; +#endif // DEBUG_URLFACTORY + QRegExp reg("\"*https://mts0.google.com/vt/lyrs=m@(\\d*)", Qt::CaseInsensitive); if (reg.indexIn(html) != -1) { QStringList gc = reg.capturedTexts(); @@ -148,6 +163,7 @@ void UrlFactory::TryCorrectGoogleVersions() qDebug() << "TryCorrectGoogleVersions, VersionGoogleLabels: " << VersionGoogleLabels; #endif // DEBUG_URLFACTORY } + reg = QRegExp("\"*https://khms0.google.com/kh/v=(\\d*)", Qt::CaseInsensitive); if (reg.indexIn(html) != -1) { QStringList gc = reg.capturedTexts(); @@ -157,6 +173,7 @@ void UrlFactory::TryCorrectGoogleVersions() qDebug() << "TryCorrectGoogleVersions, VersionGoogleSatellite: " << VersionGoogleSatellite; } + reg = QRegExp("\"*https://mts0.google.com/vt/lyrs=t@(\\d*),r@(\\d*)", Qt::CaseInsensitive); if (reg.indexIn(html) != -1) { QStringList gc = reg.capturedTexts(); @@ -208,7 +225,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c QString sec2 = ""; // after &zoom=... GetSecGoogleWords(pos, sec1, sec2); TryCorrectGoogleVersions(); - + qDebug() << QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); } break; @@ -259,6 +276,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c TryCorrectGoogleVersions(); // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + qDebug() << QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); } break; @@ -318,37 +336,26 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c return QString("https://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); } break; - case MapType::YahooMap: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1)); - } - - case MapType::YahooSatellite: - { - return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; - case MapType::YahooLabels: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; + // *.yimg.com has been depreciated. "Here" is what Yahoo uses now. https://developer.here.com/rest-apis/documentation/enterprise-map-tile/topics/request-constructing.html case MapType::OpenStreetMap: { char letter = "abc"[GetServerNum(pos, 3)]; return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); } break; + // Need to update tile format to fit http://wiki.openstreetmap.org/wiki/Tile_servers case MapType::OpenStreetOsm: { char letter = "abc"[GetServerNum(pos, 3)]; - return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + qDebug() << QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); } break; case MapType::OpenStreetMapSurfer: { // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16 + qDebug() << QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); } break; @@ -356,6 +363,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c { // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14 + qDebug() << QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); } break; @@ -395,7 +403,8 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c { // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + qDebug() << QString("http://server.arcgisonline.com/ArcGIS/rest/services/World_Shaded_Relief/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/World_Shaded_Relief/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); } break; case MapType::ArcGIS_Terrain: @@ -414,6 +423,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg // TODO verificar + qDebug() << QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); } break; @@ -426,6 +436,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png // TODO verificar // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png + qDebug() << QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); } break; @@ -435,6 +446,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); // http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png // TODO verificar + qDebug() << QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); } break; @@ -455,22 +467,33 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c QString y = QString("%1").arg(QString::number(pos.Y()), 9, (QChar)'0'); y.insert(3, "/").insert(7, "/"); // "http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png" + // This has changed map03 does not exist. (neither does map3) Servers have changed to map1 and map2? return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom, 2, 10, (QChar)'0').arg(x).arg(y); } break; case MapType::SigPacSpainMap: { - return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); + // http://sigpac.magrama.es/fega/h5visor/ is new server location + qDebug() << QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); + return QString("http://sigpac.magrama.es/SDG/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); } break; case MapType::YandexMapRu: { + /* + Used "oldmaps" to determine map types - https://old.maps.yandex.ru/?ll=-83.110960%2C40.091250&spn=7.745361%2C6.015476&z=7&l=map + map: 'https:\/\/vec0%d.maps.yandex.net\/tiles?l=map&%c&%l', + sat: 'https:\/\/sat0%d.maps.yandex.net\/tiles?l=sat&%c&%l', + skl: 'https:\/\/vec0%d.maps.yandex.net\/tiles?l=skl&%c&%l', + */ + QString server = "vec"; + return QString("http://%1").arg(server) + QString("0%2.maps.yandex.net/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); - // http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11 - - return QString("http://%1").arg(server) + QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); +// Satllite maps are poor quality, but available. +// QString server = "sat"; +// return QString("http://%1").arg(server) + QString("0%2.maps.yandex.net/tiles?l=sat&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); } break; default: @@ -492,23 +515,29 @@ void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec QString UrlFactory::MakeGeocoderUrl(QString keywords) { QString key = keywords.replace(' ', '+'); - - return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey); + // CSV output has been depreciated. API key is no longer needed. + return QString("https://maps.googleapis.com/maps/api/geocode/xml?sensor=false&address=%1").arg(key); } QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language) { - return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey); + // CSV output has been depreciated. API key is no longer needed. + return QString("https://maps.googleapis.com/maps/api/geocode/xml?latlng=%1,%2").arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())); } -internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status) +internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, QString &status) { return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords), UseGeocoderCache, status); } -internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status) + +QString latxml; +QString lonxml; +//QString status; + +internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, QString &status) { #ifdef DEBUG_URLFACTORY qDebug() << "Entered GetLatLngFromGeocoderUrl:"; #endif // DEBUG_URLFACTORY - status = GeoCoderStatusCode::Unknow; + status = "ZERO_RESULTS"; internals::PointLatLng ret(0, 0); QString urlEnd = url.mid(url.indexOf("geo?q=") + 6); urlEnd.replace(QRegExp( @@ -529,6 +558,10 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, #endif // DEBUG_URLFACTORY QNetworkReply *reply; QNetworkRequest qheader; + // Lame hack *SSL security == none, bypass QT bug + QSslConfiguration conf = qheader.sslConfiguration(); + conf.setPeerVerifyMode(QSslSocket::VerifyNone); + qheader.setSslConfiguration(conf); QNetworkAccessManager network; network.setProxy(Proxy); qheader.setUrl(QUrl(url)); @@ -552,11 +585,86 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, return internals::PointLatLng(0, 0); } { + geo = reply->readAll(); #ifdef DEBUG_URLFACTORY qDebug() << "GetLatLngFromGeocoderUrl:Reply ok"; + qDebug() << geo; // This is the response from the geocode request (no longer in CSV) #endif // DEBUG_URLFACTORY - geo = reply->readAll(); + // This is SOOOO horribly hackish, code duplication needs to go. Needed a quick fix. + QXmlStreamReader reader(geo); + while(!reader.atEnd()) + { + reader.readNext(); + + if(reader.isStartElement()) + { + if(reader.name() == "lat") + { + reader.readNext(); + if(reader.atEnd()) + break; + + if(reader.isCharacters()) + { + QString text = reader.text().toString(); + qDebug() << text; + latxml = text; + break; + } + } + } + + } + + while(!reader.atEnd()) + { + reader.readNext(); + + if(reader.isStartElement()) + { + if(reader.name() == "lng") + { + reader.readNext(); + if(reader.atEnd()) + break; + + if(reader.isCharacters()) + { + QString text = reader.text().toString(); + qDebug() << text; + lonxml = text; + break; + } + } + } + + } + + QXmlStreamReader reader2(geo); + while(!reader2.atEnd()) + { + reader2.readNext(); + + if(reader2.isStartElement()) + { + if(reader2.name() == "status") + { + reader2.readNext(); + if(reader2.atEnd()) + break; + + if(reader2.isCharacters()) + { + QString text = reader2.text().toString(); + qDebug() << text; + status = text; + break; + } + } + } + + } // cache geocoding if (useCache && geo.startsWith("200")) { @@ -567,23 +675,36 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, } - // parse values - // true : 200,4,56.1451640,22.0681787 - // false: 602,0,0,0 { - QStringList values = geo.split(','); - if (values.count() == 4) { - status = (GeoCoderStatusCode::Types)QString(values[0]).toInt(); - if (status == GeoCoderStatusCode::G_GEO_SUCCESS) { - double lat = QString(values[2]).toDouble(); - double lng = QString(values[3]).toDouble(); + if (status == "OK") { + double lat = QString(latxml).toDouble(); + double lng = QString(lonxml).toDouble(); ret = internals::PointLatLng(lat, lng); #ifdef DEBUG_URLFACTORY + qDebug() << "Status is: " << status; qDebug() << "Lat=" << lat << " Lng=" << lng; #endif // DEBUG_URLFACTORY } - } + else if (status == "ZERO_RESULTS") { + qDebug() << "No results"; + } + else if (status == "OVER_QUERY_LIMIT") { + qDebug() << "You are over quota on queries"; + } + else if (status == "REQUEST_DENIED") { + qDebug() << "Request was denied"; + } + else if (status == "INVALID_REQUEST") { + qDebug() << "Invalid request, missing address, lat long or location"; + } + else if (status == "UNKNOWN_ERROR") { + qDebug() << "Some sort of server error."; + } + else + { + qDebug() << "UrlFactory loop error"; + } } return ret; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h index ace8b52d3..76de69f0b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h @@ -56,7 +56,7 @@ public: UrlFactory(); ~UrlFactory(); QString MakeImageUrl(const MapType::Types &type, const core::Point &pos, const int &zoom, const QString &language); - internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status); + internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, QString &status); Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location); int Timeout; private: @@ -79,7 +79,7 @@ protected: void setIsCorrectGoogleVersions(bool value); QString MakeGeocoderUrl(QString keywords); QString MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language); - internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status); + internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, QString &status); Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache); }; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp index b56d84e75..f85b17958 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp @@ -377,15 +377,18 @@ void Core::OnMapClose() CancelAsyncTasks(); } -GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const & keys) +QString Core::SetCurrentPositionByKeywords(QString const & keys) { - GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow; + QString status = "ZERO_RESULTS"; PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status); - if (!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) { + if (!pos.IsEmpty() && (status == "OK")) { SetCurrentPosition(pos); } - + else + { + qDebug() << "Status is not OK: " << status; + } return status; } RectLatLng Core::CurrentViewArea() diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h index 9998f0408..7bcae973b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h @@ -266,7 +266,7 @@ public: void OnMapClose(); // TODO had as slot - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys); + QString SetCurrentPositionByKeywords(QString const & keys); RectLatLng CurrentViewArea(); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h index e1b2c5d88..463800fb1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h @@ -229,7 +229,7 @@ private: { core->ReloadMap(); } - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + QString SetCurrentPositionByKeywords(QString const & keys) { return core->SetCurrentPositionByKeywords(keys); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index 005509908..895baf452 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -350,7 +350,7 @@ public: map->ReloadMap(); map->resize(); } - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + QString SetCurrentPositionByKeywords(QString const & keys) { return map->SetCurrentPositionByKeywords(keys); } diff --git a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui deleted file mode 100644 index e7c7ddf10..000000000 --- a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui +++ /dev/null @@ -1,658 +0,0 @@ - - - CC_HW_Widget - - - - 0 - 0 - 646 - 596 - - - - Form - - - - - - 0 - - - - HW settings - - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 232 - 232 - 232 - - - - - - - 232 - 232 - 232 - - - - - - - - QFrame::NoFrame - - - QFrame::Plain - - - true - - - - - 0 - 0 - 624 - 510 - - - - - 12 - - - 12 - - - 12 - - - 12 - - - - - - 0 - 0 - - - - - 0 - 50 - - - - Messages - - - - - - - 75 - true - - - - - - - Qt::AutoText - - - true - - - - - - - - - - - 0 - 0 - - - - - 500 - 0 - - - - - 500 - 16777215 - - - - - 75 - true - - - - Changes on this page only take effect after board reset or power cycle - - - Qt::AlignCenter - - - true - - - - - - - Qt::Vertical - - - - 20 - 25 - - - - - - - - - - - - Select the speed here. - - - - - - - - 55 - 0 - - - - GPS speed: - - - - - - - Telemetry speed: - - - - - - - Select the speed here. - - - - - - - Select the speed here. - - - - - - - - 55 - 0 - - - - ComUsbBridge speed: - - - - - - - GPS protocol : - - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - - - - - - USB HID Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - - - - - USB VCP Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - false - - - - - - - Main Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - Flexi Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - - Receiver Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 10 - - - - - - - - - - - - - - - - 4 - - - - - Qt::Horizontal - - - - 369 - 20 - - - - - - - - - 0 - 0 - - - - - 25 - 25 - - - - - 25 - 25 - - - - Takes you to the wiki page - - - - - - - :/core/images/helpicon.svg:/core/images/helpicon.svg - - - - 25 - 25 - - - - true - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 232 - 232 - 232 - - - - - - - 232 - 232 - 232 - - - - - - - - Send to OpenPilot but don't write in SD. -Beware of not locking yourself out! - - - false - - - - - - Apply - - - - 16 - 16 - - - - false - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Applies and Saves all settings to SD. -Beware of not locking yourself out! - - - false - - - - - - Save - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui deleted file mode 100644 index 1a659982e..000000000 --- a/ground/openpilotgcs/src/plugins/config/ccattitude.ui +++ /dev/null @@ -1,597 +0,0 @@ - - - ccattitude - - - - 0 - 0 - 780 - 566 - - - - Form - - - - - - 0 - - - - Attitude - - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 255 - 255 - 255 - - - - - - - 232 - 232 - 232 - - - - - - - - - 232 - 232 - 232 - - - - - - - 232 - 232 - 232 - - - - - - - - QFrame::NoFrame - - - QFrame::Plain - - - true - - - - - 0 - 0 - 758 - 486 - - - - - 12 - - - 12 - - - 12 - - - 12 - - - - - Rotate virtual attitude relative to board - - - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Roll - - - Qt::AlignCenter - - - - - - - -180 - - - 180 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - -180 - - - 180 - - - - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Yaw - - - Qt::AlignCenter - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Pitch - - - Qt::AlignCenter - - - - - - - -90 - - - 90 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - Calibration - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Place aircraft very flat, and then click level to compute the accelerometer and gyro bias - - - true - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - 500 - 16777215 - - - - Launch horizontal calibration. - - - Level - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - - - - - - 300 - 0 - - - - 0 - - - - - - - - - If enabled, a fast recalibration of gyro zero point will be done -whenever the frame is armed. Do not move the airframe while -arming it in that case! - - - Zero gyros while arming aircraft - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - Filtering - - - - - - - 0 - 0 - - - - - - - Accelerometers - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 0 - - - - - - - - - 60 - 0 - - - - Accelerometer filtering. - -Sets the amount of lowpass filtering of accelerometer data -for the attitude estimation. Higher values apply a stronger -filter, which may help with drifting in attitude mode. - -Range: 0.00 - 0.20, Good starting value: 0.05 - 0.10 -Start low and raise until drift stops. - -A setting of 0.00 disables the filter. - - - 2 - - - 0.200000000000000 - - - 0.010000000000000 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - Qt::Vertical - - - - 20 - 10 - - - - - - - - - - - - - - - - 4 - - - - - Qt::Horizontal - - - - 380 - 20 - - - - - - - - - 0 - 0 - - - - - 25 - 25 - - - - Takes you to the wiki page - - - - - - - :/core/images/helpicon.svg:/core/images/helpicon.svg - - - - 25 - 25 - - - - true - - - - - - - - 0 - 0 - - - - Apply - - - - - - - - 0 - 0 - - - - Click to permanently save the accel bias in the CopterControl Flash. - - - Save - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 18c0984ea..30199471c 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -21,8 +21,6 @@ HEADERS += \ configinputwidget.h \ configoutputwidget.h \ configvehicletypewidget.h \ - config_cc_hw_widget.h \ - configccattitudewidget.h \ configstabilizationwidget.h \ assertions.h \ defaultattitudewidget.h \ @@ -68,8 +66,6 @@ SOURCES += \ configinputwidget.cpp \ configoutputwidget.cpp \ configvehicletypewidget.cpp \ - config_cc_hw_widget.cpp \ - configccattitudewidget.cpp \ configstabilizationwidget.cpp \ defaultattitudewidget.cpp \ defaulthwsettingswidget.cpp \ @@ -106,12 +102,10 @@ FORMS += \ airframe_ground.ui \ airframe_multirotor.ui \ airframe_custom.ui \ - cc_hw_settings.ui \ stabilization.ui \ input.ui \ input_wizard.ui \ output.ui \ - ccattitude.ui \ defaultattitude.ui \ defaulthwsettings.ui \ inputchannelform.ui \ @@ -121,6 +115,6 @@ FORMS += \ txpid.ui \ mixercurve.ui \ configrevohwwidget.ui \ - oplink.ui + oplink.ui RESOURCES += configgadget.qrc diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp deleted file mode 100644 index 4fe823e6a..000000000 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/** - ****************************************************************************** - * - * @file configtelemetrywidget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup ConfigPlugin Config Plugin - * @{ - * @brief The Configuration Gadget used to update settings in the firmware - *****************************************************************************/ -/* - * 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 "config_cc_hw_widget.h" -#include "hwsettings.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) -{ - m_telemetry = new Ui_CC_HW_Widget(); - m_telemetry->setupUi(this); - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings *settings = pm->getObject(); - if (!settings->useExpertMode()) { - m_telemetry->saveTelemetryToRAM->setVisible(false); - } - - - UAVObjectUtilManager *utilMngr = pm->getObject(); - int id = utilMngr->getBoardModel(); - - switch (id) { - case 0x0101: - m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0101.svg")); - break; - case 0x0301: - m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0301.svg")); - break; - case 0x0401: - m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); - break; - case 0x0402: - m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); - break; - case 0x0201: - m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0201.svg")); - break; - default: - m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); - break; - } - addApplySaveButtons(m_telemetry->saveTelemetryToRAM, m_telemetry->saveTelemetryToSD); - addWidgetBinding("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi); - addWidgetBinding("HwSettings", "CC_MainPort", m_telemetry->cbTele); - addWidgetBinding("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr); - addWidgetBinding("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid); - addWidgetBinding("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp); - addWidgetBinding("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed); - addWidgetBinding("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed); - // Add Gps protocol configuration - - HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); - HwSettings::DataFields hwSettingsData = hwSettings->getData(); - - if (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) { - m_telemetry->gpsProtocol->setEnabled(false); - m_telemetry->gpsProtocol->setToolTip(tr("Enable GPS module and reboot the board to be able to select GPS protocol")); - } else { - addWidgetBinding("GPSSettings", "DataProtocol", m_telemetry->gpsProtocol); - } - - addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed); - connect(m_telemetry->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - enableSaveButtons(false); - populateWidgets(); - refreshWidgetsValues(); - forceConnectedState(); -} - -ConfigCCHWWidget::~ConfigCCHWWidget() -{ - // Do nothing -} - -void ConfigCCHWWidget::refreshValues() -{} - -void ConfigCCHWWidget::widgetsContentsChanged() -{ - ConfigTaskWidget::widgetsContentsChanged(); - - if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && - (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && - (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && - (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) { - enableSaveButtons(false); - m_telemetry->problems->setText(tr("Warning: you have configured more than one DebugConsole, this currently is not supported")); - } else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) { - enableSaveButtons(false); - m_telemetry->problems->setText(tr("Warning: you have configured both MainPort and FlexiPort for the same function, this currently is not supported")); - } else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) { - enableSaveButtons(false); - m_telemetry->problems->setText(tr("Warning: you have configured both USB HID Port and USB VCP Port for the same function, this currently is not supported")); - } else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) { - enableSaveButtons(false); - m_telemetry->problems->setText(tr("Warning: you have disabled USB Telemetry on both USB HID Port and USB VCP Port, this currently is not supported")); - } else { - m_telemetry->problems->setText(""); - enableSaveButtons(true); - } -} - -void ConfigCCHWWidget::enableSaveButtons(bool enable) -{ - m_telemetry->saveTelemetryToRAM->setEnabled(enable); - m_telemetry->saveTelemetryToSD->setEnabled(enable); -} - -void ConfigCCHWWidget::openHelp() -{ - QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/x/D4AUAQ"), QUrl::StrictMode)); -} - -/** - * @} - * @} - */ diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h deleted file mode 100644 index 313231382..000000000 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - ****************************************************************************** - * - * @file configtelemetrytwidget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup ConfigPlugin Config Plugin - * @{ - * @brief Telemetry configuration panel - *****************************************************************************/ -/* - * 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 CONFIGCCHWWIDGET_H -#define CONFIGCCHWWIDGET_H - -#include "ui_cc_hw_settings.h" -#include "../uavobjectwidgetutils/configtaskwidget.h" -#include "extensionsystem/pluginmanager.h" -#include "uavobjectmanager.h" -#include "uavobject.h" -#include -#include -#include "smartsavebutton.h" - -class ConfigCCHWWidget : public ConfigTaskWidget { - Q_OBJECT - -public: - ConfigCCHWWidget(QWidget *parent = 0); - ~ConfigCCHWWidget(); -private slots: - void openHelp(); - void refreshValues(); - void widgetsContentsChanged(); - void enableSaveButtons(bool enable); - -private: - Ui_CC_HW_Widget *m_telemetry; - QSvgRenderer *m_renderer; -}; - -#endif // CONFIGCCHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp deleted file mode 100644 index 4e872735b..000000000 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ /dev/null @@ -1,239 +0,0 @@ -/** - ****************************************************************************** - * - * @file configccattitudewidget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup ConfigPlugin Config Plugin - * @{ - * @brief Configure Attitude module on CopterControl - *****************************************************************************/ -/* - * 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 "configccattitudewidget.h" -#include "ui_ccattitude.h" -#include "utils/coordinateconversions.h" -#include "attitudesettings.h" -#include -#include -#include -#include -#include -#include "accelstate.h" -#include "accelgyrosettings.h" -#include "gyrostate.h" -#include -#include -#include -ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : - ConfigTaskWidget(parent), - ui(new Ui_ccattitude) -{ - ui->setupUi(this); - connect(ui->zeroBias, SIGNAL(clicked()), this, SLOT(startAccelCalibration())); - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings *settings = pm->getObject(); - if (!settings->useExpertMode()) { - ui->applyButton->setVisible(false); - } - - addApplySaveButtons(ui->applyButton, ui->saveButton); - addUAVObject("AttitudeSettings"); - addUAVObject("AccelGyroSettings"); - - // Connect the help button - connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - - addWidgetBinding("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming); - addWidgetBinding("AttitudeSettings", "AccelTau", ui->accelTauSpinbox); - - addWidgetBinding("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL); - addWidgetBinding("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH); - addWidgetBinding("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW); - addWidget(ui->zeroBias); - populateWidgets(); - refreshWidgetsValues(); - forceConnectedState(); -} - -ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() -{ - delete ui; -} - -void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) -{ - if (!timer.isActive()) { - // ignore updates that come in after the timer has expired - return; - } - - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - - // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples - // for both gyros and accels. - // Note that, at present, we stash the samples and then compute the bias - // at the end, even though the mean could be accumulated as we go. - // In future, a better algorithm could be used. - if (obj->getObjID() == AccelState::OBJID) { - accelUpdates++; - AccelState::DataFields accelStateData = accelState->getData(); - x_accum.append(accelStateData.x); - y_accum.append(accelStateData.y); - z_accum.append(accelStateData.z); - } else if (obj->getObjID() == GyroState::OBJID) { - gyroUpdates++; - GyroState::DataFields gyroStateData = gyroState->getData(); - x_gyro_accum.append(gyroStateData.x); - y_gyro_accum.append(gyroStateData.y); - z_gyro_accum.append(gyroStateData.z); - } - - // update the progress indicator - ui->zeroBiasProgress->setValue((float)qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); - - // If we have enough samples, then stop sampling and compute the biases - if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { - timer.stop(); - disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); - disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); - - float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum); - float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum); - float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81; - - float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum); - float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum); - float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum); - accelState->setMetadata(initialAccelStateMdata); - gyroState->setMetadata(initialGyroStateMdata); - - AccelGyroSettings::DataFields accelGyroSettingsData = AccelGyroSettings::GetInstance(getObjectManager())->getData(); - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); - // We offset the gyro bias by current bias to help precision - accelGyroSettingsData.accel_bias[0] += x_bias; - accelGyroSettingsData.accel_bias[1] += y_bias; - accelGyroSettingsData.accel_bias[2] += z_bias; - accelGyroSettingsData.gyro_bias[0] = -x_gyro_bias; - accelGyroSettingsData.gyro_bias[1] = -y_gyro_bias; - accelGyroSettingsData.gyro_bias[2] = -z_gyro_bias; - attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; - AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); - AccelGyroSettings::GetInstance(getObjectManager())->setData(accelGyroSettingsData); - this->setDirty(true); - - // reenable controls - enableControls(true); - } -} - -void ConfigCCAttitudeWidget::timeout() -{ - UAVDataObject *obj = AccelState::GetInstance(getObjectManager()); - - disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); - disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); - - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - accelState->setMetadata(initialAccelStateMdata); - gyroState->setMetadata(initialGyroStateMdata); - - QMessageBox msgBox; - msgBox.setText(tr("Calibration timed out before receiving required updates.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - - // reset progress indicator - ui->zeroBiasProgress->setValue(0); - // reenable controls - enableControls(true); -} - -void ConfigCCAttitudeWidget::startAccelCalibration() -{ - // disable controls during sampling - enableControls(false); - - accelUpdates = 0; - gyroUpdates = 0; - x_accum.clear(); - y_accum.clear(); - z_accum.clear(); - x_gyro_accum.clear(); - y_gyro_accum.clear(); - z_gyro_accum.clear(); - - // Disable gyro bias correction to see raw data - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); - attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; - AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); - - // Set up to receive updates - UAVDataObject *accelState = AccelState::GetInstance(getObjectManager()); - UAVDataObject *gyroState = GyroState::GetInstance(getObjectManager()); - connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); - connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); - - // Speed up updates - initialAccelStateMdata = accelState->getMetadata(); - UAVObject::Metadata accelStateMdata = initialAccelStateMdata; - UAVObject::SetFlightTelemetryUpdateMode(accelStateMdata, UAVObject::UPDATEMODE_PERIODIC); - accelStateMdata.flightTelemetryUpdatePeriod = 30; // ms - accelState->setMetadata(accelStateMdata); - - initialGyroStateMdata = gyroState->getMetadata(); - UAVObject::Metadata gyroStateMdata = initialGyroStateMdata; - UAVObject::SetFlightTelemetryUpdateMode(gyroStateMdata, UAVObject::UPDATEMODE_PERIODIC); - gyroStateMdata.flightTelemetryUpdatePeriod = 30; // ms - gyroState->setMetadata(gyroStateMdata); - - // Set up timeout timer - timer.setSingleShot(true); - timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelStateMdata.flightTelemetryUpdatePeriod, - gyroStateMdata.flightTelemetryUpdatePeriod))); - connect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); -} - -void ConfigCCAttitudeWidget::openHelp() -{ - QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/x/44Cf"), QUrl::StrictMode)); -} - -void ConfigCCAttitudeWidget::setAccelFiltering(bool active) -{ - Q_UNUSED(active); - setDirty(true); -} - -void ConfigCCAttitudeWidget::enableControls(bool enable) -{ - ui->zeroBias->setEnabled(enable); - ui->zeroGyroBiasOnArming->setEnabled(enable); - ui->accelTauSpinbox->setEnabled(enable); - ConfigTaskWidget::enableControls(enable); -} - -void ConfigCCAttitudeWidget::updateObjectsFromWidgets() -{ - ConfigTaskWidget::updateObjectsFromWidgets(); - - ui->zeroBiasProgress->setValue(0); -} diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h deleted file mode 100644 index b4d84a5b9..000000000 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ /dev/null @@ -1,74 +0,0 @@ -/** - ****************************************************************************** - * - * @file configccattitudewidget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup ConfigPlugin Config Plugin - * @{ - * @brief Configure the properties of the attitude module in CopterControl - *****************************************************************************/ -/* - * 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 CCATTITUDEWIDGET_H -#define CCATTITUDEWIDGET_H - -#include "ui_ccattitude.h" -#include "../uavobjectwidgetutils/configtaskwidget.h" -#include "extensionsystem/pluginmanager.h" -#include "uavobjectmanager.h" -#include "uavobject.h" -#include -#include - -class Ui_Widget; - -class ConfigCCAttitudeWidget : public ConfigTaskWidget { - Q_OBJECT - -public: - explicit ConfigCCAttitudeWidget(QWidget *parent = 0); - ~ConfigCCAttitudeWidget(); - - virtual void updateObjectsFromWidgets(); - -private slots: - void sensorsUpdated(UAVObject *obj); - void timeout(); - void startAccelCalibration(); - void openHelp(); - void setAccelFiltering(bool active); - -private: - Ui_ccattitude *ui; - QTimer timer; - UAVObject::Metadata initialAccelStateMdata; - UAVObject::Metadata initialGyroStateMdata; - - int accelUpdates; - int gyroUpdates; - - QList x_accum, y_accum, z_accum; - QList x_gyro_accum, y_gyro_accum, z_gyro_accum; - - static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1; - static const int NUM_SENSOR_UPDATES = 300; -protected: - virtual void enableControls(bool enable); -}; - -#endif // CCATTITUDEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index d89ac5bca..856c5edd4 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -12,9 +12,7 @@ images/fixedwing-shapes.svg images/ground-shapes.svg images/ccpm_setup.svg - images/PipXtreme.png images/help.png - images/coptercontrol.svg images/TX2.svg images/output_selected.png images/output_normal.png diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index a611c6c51..51e69d562 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -28,14 +28,12 @@ #include "configgadgetwidget.h" #include "configvehicletypewidget.h" -#include "configccattitudewidget.h" #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" #include "configcamerastabilizationwidget.h" #include "configtxpidwidget.h" #include "configrevohwwidget.h" -#include "config_cc_hw_widget.h" #include "configoplinkwidget.h" #include "configrevowidget.h" #include "defaultattitudewidget.h" @@ -183,19 +181,13 @@ void ConfigGadgetWidget::onAutopilotConnect() UAVObjectUtilManager *utilMngr = pm->getObject(); if (utilMngr) { int board = utilMngr->getBoardModel(); - if ((board & 0xff00) == 1024) { - // CopterControl family - QWidget *qwd = new ConfigCCAttitudeWidget(this); - stackWidget->replaceTab(ConfigGadgetWidget::sensors, qwd); - - qwd = new ConfigCCHWWidget(this); - stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd); - } else if ((board & 0xff00) == 0x0900) { + if ((board & 0xff00) == 0x0900) { // Revolution family QWidget *qwd = new ConfigRevoWidget(this); stackWidget->replaceTab(ConfigGadgetWidget::sensors, qwd); - - qwd = new ConfigRevoHWWidget(this); + if (board == 0x0903) { + qwd = new ConfigRevoHWWidget(this); + } stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd); } else { // Unknown board diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index 33c6466b7..9159f5315 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -354,11 +354,7 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj) if (utilMngr) { int board = utilMngr->getBoardModel(); // Setup labels and combos for banks according to board type - if ((board & 0xff00) == 0x0400) { - // Coptercontrol family of boards 4 timer banks - bankLabels << "1 (1-3)" << "2 (4)" << "3 (5,7-8)" << "4 (6,9-10)"; - channelBanks << 1 << 1 << 1 << 2 << 3 << 4 << 3 << 3 << 4 << 4; - } else if ((board & 0xff00) == 0x0900) { + if ((board & 0xff00) == 0x0900) { // Revolution family of boards 6 timer banks bankLabels << "1 (1-2)" << "2 (3)" << "3 (4)" << "4 (5-6)" << "5 (7-8)" << "6 (9-10)"; channelBanks << 1 << 1 << 2 << 3 << 4 << 4 << 5 << 5 << 6 << 6; diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index ae224bd6c..5a0258da8 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -151,6 +151,12 @@ void ConfigRevoHWWidget::usbVCPPortChanged(int index) m_ui->cbMain->model()->setData(m_ui->cbMain->model()->index(HwSettings::RM_MAINPORT_COMBRIDGE, 0), !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); + if (!vcpComBridgeEnabled && m_ui->cbRcvr->currentIndex() == HwSettings::RM_RCVRPORT_COMBRIDGE) { + m_ui->cbRcvr->setCurrentIndex(HwSettings::RM_RCVRPORT_DISABLED); + } + m_ui->cbRcvr->model()->setData(m_ui->cbRcvr->model()->index(HwSettings::RM_RCVRPORT_COMBRIDGE, 0), + !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); + // _DEBUGCONSOLE modes are mutual exclusive if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { diff --git a/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png b/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png deleted file mode 100644 index cd9396e6c..000000000 Binary files a/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png and /dev/null differ diff --git a/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg b/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg deleted file mode 100644 index 669199ef6..000000000 --- a/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg +++ /dev/null @@ -1,2647 +0,0 @@ - - - -image/svg+xml \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index a2a157089..aa5b3b553 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -7,7 +7,7 @@ 0 0 974 - 857 + 755 @@ -136,8 +136,8 @@ 0 0 - 939 - 776 + 950 + 775 @@ -8244,8 +8244,8 @@ border-radius: 5; 0 0 - 952 - 763 + 950 + 736 @@ -18236,8 +18236,8 @@ border-radius: 5; 0 0 - 952 - 763 + 950 + 671 @@ -24082,8 +24082,8 @@ font:bold; 0 0 - 952 - 763 + 950 + 671 @@ -25209,8 +25209,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:AltitudePI - element:Kp + fieldname:VerticalPosP scale:0.01 haslimits:yes buttongroup:98,10 @@ -25294,15 +25293,18 @@ border-radius: 5; - + - <html><head/><body><p>How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html> + <html><head/><body><p>How fast the vehicle should attain its target velocity. For neutral throttle estimation, the altitude module assumes that when engaged the throttle thrust limit neutral setting is in the range required to hover. If the throttle required is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot;. Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html> - 1000 + 100 + + + 1 - 50 + 25 Qt::Horizontal @@ -25313,17 +25315,17 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI + fieldname:VerticalVelPID element:Ki - scale:0.00001 + scale:0.01 haslimits:yes buttongroup:98 - - + + 0 @@ -25355,13 +25357,13 @@ border-radius: 5; 100 - 51 + 25 objname:AltitudeHoldSettings - fieldname:VelocityPI - element:Kp + fieldname:VerticalVelPID + element:Ki scale:0.01 haslimits:yes buttongroup:98,10 @@ -25369,8 +25371,68 @@ border-radius: 5; - - + + + + + 0 + 0 + + + + + 58 + 0 + + + + + 16777215 + 16777215 + + + + + + + Velocity Derivative + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <html><head/><body><p>Small abouts of Kd can reduce oscillations in the velocity controller.</p></body></html> + + + 300 + + + 50 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Kd + scale:0.0001 + haslimits:yes + buttongroup:98 + + + + + + 0 @@ -25399,7 +25461,7 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - 1000 + 300 51 @@ -25407,9 +25469,172 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI - element:Ki - scale:0.00001 + fieldname:VerticalVelPID + element:Kd + scale:0.0001 + haslimits:yes + buttongroup:98,10 + + + + + + + + + 0 + 0 + + + + + 58 + 0 + + + + + 16777215 + 16777215 + + + + + + + Velocity Beta + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <html><head/><body><p>The beta value applies a setpoint weighting that reduces the sensitivity to quick changes in the desired velocity. Transitions from altitude hold to descent/climb can be made smooth applying a Beta value of around 80 to 90%.</p></body></html> + + + 70 + + + 100 + + + 90 + + + 90 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Beta + scale:0.01 + haslimits:yes + buttongroup:98 + + + + + + + + + 0 + 0 + + + + + 50 + 22 + + + + + 50 + 22 + + + + Qt::StrongFocus + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 70 + + + 100 + + + 90 + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Beta + scale:0.01 + haslimits:yes + buttongroup:98,10 + + + + + + + + + 0 + 0 + + + + + 50 + 22 + + + + + 50 + 22 + + + + Qt::StrongFocus + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 100 + + + 50 + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Kp + scale:0.01 haslimits:yes buttongroup:98,10 @@ -25419,7 +25644,7 @@ border-radius: 5; - <html><head/><body><p>How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.</p></body></html> + <html><head/><body><p>How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.</p></body></html> 100 @@ -25434,10 +25659,9 @@ border-radius: 5; QSlider::TicksBelow - + objname:AltitudeHoldSettings - fieldname:AltitudePI - element:Kp + fieldname:VerticalPosP scale:0.01 haslimits:yes buttongroup:98 @@ -25446,7 +25670,7 @@ border-radius: 5; - + <html><head/><body><p>How much the vehicle should throttle up or down to compensate or achieve a certain vertical speed. Higher values lead to more aggressive throttle changes and could lead to oscillations. This is the most likely candidate to change depending on the crafts engine thrust. Heavy craft with weak engines might require higher values.</p></body></html> @@ -25465,7 +25689,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI + fieldname:VerticalVelPID element:Kp scale:0.01 haslimits:yes @@ -27173,10 +27397,10 @@ Useful if you have accidentally changed some settings. pushButton_7 AltKpSlider AltKp - AltKiSlider - AltKi - AltKdSlider - AltKd + VelKpSlider + VelKp + VelKiSlider + VelKi pushButton_8 AltThrExpSlider_2 AltThrExp_2 diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index a225b97e6..ed0599128 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -89,6 +89,8 @@ void ConnectionManager::init() // new connection object from plugins QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), this, SLOT(objectAdded(QObject *))); QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), this, SLOT(aboutToRemoveObject(QObject *))); + + ccWarningClosed = true; } @@ -435,16 +437,23 @@ void ConnectionManager::devChanged(IConnection *connection) } // clear device list combobox m_availableDevList->clear(); + ccFound = false; // remove registered devices of this IConnection from the list updateConnectionList(connection); updateConnectionDropdown(); + if (ccFound && ccWarningClosed) { + ccWarningClosed = false; + QMessageBox::information(this, tr("CopterControl Not Supported"), + tr("This version of OpenPilot GCS does not support CC and CC3D boards.\n\nPlease use OpenPilot GCS version 15.02.xx instead")); + ccWarningClosed = true; + } + qDebug() << "# devices " << m_devList.count(); emit availableDevicesChanged(m_devList); - // disable connection button if the liNameif (m_availableDevList->count() > 0) if (m_availableDevList->count() > 0) { m_connectBtn->setEnabled(true); @@ -457,17 +466,21 @@ void ConnectionManager::updateConnectionDropdown() { // add all the list again to the combobox foreach(DevListItem d, m_devList) { - m_availableDevList->addItem(d.getConName()); - m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); - if (!m_ioDev && d.getConName().startsWith("USB")) { - if (m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) { - m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); - } - if (m_mainWindow->generalSettings()->autoConnect() && polling) { - qDebug() << "Automatically opening device"; - connectDevice(d); - qDebug() << "ConnectionManager::updateConnectionDropdown autoconnected USB device"; + if (!d.getConName().contains("CopterControl")) { + m_availableDevList->addItem(d.getConName()); + m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); + if (!m_ioDev && d.getConName().startsWith("USB")) { + if (m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) { + m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); + } + if (m_mainWindow->generalSettings()->autoConnect() && polling) { + qDebug() << "Automatically opening device"; + connectDevice(d); + qDebug() << "ConnectionManager::updateConnectionDropdown autoconnected USB device"; + } } + } else { + ccFound = true; } } if (m_ioDev) { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index b19cc7c5d..1b133d831 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -38,6 +38,7 @@ #include #include #include +#include #include "core_global.h" #include @@ -149,6 +150,8 @@ protected: private: bool connectDevice(); bool polling; + bool ccFound; + bool ccWarningClosed; Internal::MainWindow *m_mainWindow; QList connectionBackup; QTimer *reconnect; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 7fc46b3a3..3710fadaf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -134,7 +134,7 @@ MainWindow::MainWindow() : #endif m_toggleFullScreenAction(0) { - setWindowTitle(tr("OpenPilot GCS ")+VersionInfo::label()); + setWindowTitle(tr("OpenPilot GCS ") + VersionInfo::label()); #ifndef Q_WS_MAC qApp->setWindowIcon(QIcon(":/core/images/openpilot_logo_128.png")); #endif diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index 22df94b72..1e828f450 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -30,7 +30,8 @@ // #include // #include -#include "xplanesimulator.h" +#include "xplanesimulator9.h" +#include "xplanesimulator10.h" #include "hitlnoisegeneration.h" #include "extensionsystem/pluginmanager.h" #include diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp index 0ce9e9141..7760a8f0c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp @@ -32,7 +32,8 @@ #include "aerosimrcsimulator.h" #include "fgsimulator.h" #include "il2simulator.h" -#include "xplanesimulator.h" +#include "xplanesimulator9.h" +#include "xplanesimulator10.h" QList HITLPlugin::typeSimulators; @@ -57,7 +58,8 @@ bool HITLPlugin::initialize(const QStringList & args, QString *errMsg) addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); addSimulator(new FGSimulatorCreator("FG", "FlightGear")); addSimulator(new IL2SimulatorCreator("IL2", "IL2")); - addSimulator(new XplaneSimulatorCreator("X-Plane", "X-Plane")); + addSimulator(new XplaneSimulatorCreator9("X-Plane9", "X-Plane9")); + addSimulator(new XplaneSimulatorCreator10("X-Plane10", "X-Plane10")); return true; } diff --git a/ground/openpilotgcs/src/plugins/hitl/plugin.pro b/ground/openpilotgcs/src/plugins/hitl/plugin.pro index 066c60ec0..3b79330a5 100644 --- a/ground/openpilotgcs/src/plugins/hitl/plugin.pro +++ b/ground/openpilotgcs/src/plugins/hitl/plugin.pro @@ -16,7 +16,8 @@ HEADERS += hitlplugin.h \ aerosimrcsimulator.h \ fgsimulator.h \ il2simulator.h \ - xplanesimulator.h + xplanesimulator9.h \ + xplanesimulator10.h SOURCES += hitlplugin.cpp \ hitlwidget.cpp \ hitloptionspage.cpp \ @@ -28,7 +29,8 @@ SOURCES += hitlplugin.cpp \ aerosimrcsimulator.cpp \ fgsimulator.cpp \ il2simulator.cpp \ - xplanesimulator.cpp + xplanesimulator9.cpp \ + xplanesimulator10.cpp OTHER_FILES += hitl.pluginspec FORMS += hitloptionspage.ui \ hitlwidget.ui diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp new file mode 100644 index 000000000..ada7726e1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp @@ -0,0 +1,360 @@ +/** + ****************************************************************************** + * + * @file xplanesimulator10.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * 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 + */ + +/** + * Description of X-Plane Protocol: + * + * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu + * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set. + * Data Set shown all X-Plane params, + * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen + * All the UDP messages for X-Plane have the same format, which is: + * 5-character MESSAGE PROLOUGE (to indicate the type of message) + * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive) + * + * DATA INPUT/OUTPUT STRUCTURE is the following stuct: + * + * struct data_struct + * { + * int index; // data index, the index into the list of variables + // you can output from the Data Output screen in X-Plane. + * float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection.. + // many outputs do not use all 8, though. + * }; + * + * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set) + * bytes value description + * [0-3] DATA message type + * [4] none no matter + * [5-8] 11 code of setting param(row in Data Set) + * [9-41] data message data (8 float values) + * total size: 41 byte + * + */ + +#include "xplanesimulator10.h" +#include "extensionsystem/pluginmanager.h" +#include +#include +#include + +void TraceBuf10(const char *buf, int len); + +XplaneSimulator10::XplaneSimulator10(const SimulatorSettings & params) : + Simulator(params) +{ + resetInitialHomePosition(); +} + + +XplaneSimulator10::~XplaneSimulator10() +{} + +void XplaneSimulator10::setupUdpPorts(const QString & host, int inPort, int outPort) +{ + Q_UNUSED(outPort); + + inSocket->bind(QHostAddress(host), inPort); + // outSocket->bind(QHostAddress(host), outPort); + resetInitialHomePosition(); +} + +bool XplaneSimulator10::setupProcess() +{ + emit processOutput(QString("Please start X-Plane 10 manually, and make sure it is setup to output its ") + + "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); + + return true; +} + +/** + * update data in X-Plane simulator + */ +void XplaneSimulator10::transmitUpdate() +{ + if (settings.manualControlEnabled) { + // Read ActuatorDesired from autopilot + ActuatorDesired::DataFields actData = actDesired->getData(); + float ailerons = actData.Roll; + float elevator = actData.Pitch; + float rudder = actData.Yaw; + float throttle = actData.Thrust > 0 ? actData.Thrust : 0; + float none = -999; + // quint32 none = *((quint32*)&tmp); // get float as 4 bytes + + quint32 code; + QByteArray buf; + QDataStream stream(&buf, QIODevice::ReadWrite); + + // !!! LAN byte order - Big Endian +#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN + stream.setByteOrder(QDataStream::LittleEndian); +#endif + + // 11th data settings (flight con: ail/elv/rud) + buf.clear(); + code = 11; + // quint8 header[] = "DATA"; + /* + stream << *((quint32*)header) << + (quint8)0x30 << + code << + *((quint32*)&elevator) << + *((quint32*)&ailerons) << + *((quint32*)&rudder) << + none << + *((quint32*)&ailerons) << + none << + none << + none; + */ + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); +// TraceBuf10(buf.data(),41); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } + // outSocket->write(buf); + + // 25th data settings (throttle command) + buf.clear(); + code = 25; + // stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } + + // outSocket->write(buf); + + + /** !!! this settings was given from ardupilot X-Plane.pl, I comment them + but if it needed comment should be removed !!! + + // 8th data settings (joystick 1 ail/elv/rud) + stream << "DATA0" << quint32(11) << elevator << ailerons << rudder + << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); + outSocket->write(buf); + */ + } +} + +/** + * process data string from X-Plane simulator + */ +void XplaneSimulator10::processUpdate(const QByteArray & dataBuf) +{ + float altitude_msl = 0; + float altitude_agl = 0; + float latitude = 0; + float longitude = 0; + float airspeed_keas = 0; + float groundspeed_ktgs = 0; + float pitch = 0; + float roll = 0; + float heading = 0; + float pressure = 0; + float temperature = 0; + float velX = 0; + float velY = 0; + float velZ = 0; + float dstX = 0; + float dstY = 0; + float dstZ = 0; + float accX = 0; + float accY = 0; + float accZ = 0; + float rollRate_rad = 0; + float pitchRate_rad = 0; + float yawRate_rad = 0; + + // QString str; + QByteArray & buf = const_cast(dataBuf); + QString data(buf); + + if (data.left(4) == "DATA") { // check type of packet + buf.remove(0, 5); + if (dataBuf.size() % 36) { + qDebug() << ("incorrect length of UDP packet: " + buf); + return; // incorrect length of struct + } + // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 + int channelCounter = dataBuf.size() / 36; + do { + switch (buf[0]) { // switch by id + case XplaneSimulator10::LatitudeLongitude: + latitude = *((float *)(buf.data() + 4 * 1)); + longitude = *((float *)(buf.data() + 4 * 2)); + altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M; + altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M; + break; + + case XplaneSimulator10::Speed: + airspeed_keas = *((float *)(buf.data() + 4 * 2)); + groundspeed_ktgs = *((float *)(buf.data() + 4 * 4)); + break; + + case XplaneSimulator10::PitchRollHeading: + pitch = *((float *)(buf.data() + 4 * 1)); + roll = *((float *)(buf.data() + 4 * 2)); + heading = *((float *)(buf.data() + 4 * 3)); + break; + + /* + case XplaneSimulator10::SystemPressures: + pressure = *((float*)(buf.data()+4*1)); + break; + */ + + case XplaneSimulator10::AtmosphereWeather: + pressure = *((float *)(buf.data() + 4 * 1)) * INHG2KPA; + temperature = *((float *)(buf.data() + 4 * 2)); + break; + + case XplaneSimulator10::LocVelDistTraveled: + dstX = *((float *)(buf.data() + 4 * 1)); + dstY = -*((float *)(buf.data() + 4 * 3)); + dstZ = *((float *)(buf.data() + 4 * 2)); + velX = *((float *)(buf.data() + 4 * 4)); + velY = -*((float *)(buf.data() + 4 * 6)); + velZ = *((float *)(buf.data() + 4 * 5)); + break; + + case XplaneSimulator10::AngularVelocities: // In [rad/s] + pitchRate_rad = *((float *)(buf.data() + 4 * 1)); + rollRate_rad = *((float *)(buf.data() + 4 * 2)); + yawRate_rad = *((float *)(buf.data() + 4 * 3)); + break; + + case XplaneSimulator10::Gload: + accX = *((float *)(buf.data() + 4 * 6)) * GEE; + accY = *((float *)(buf.data() + 4 * 7)) * GEE; + accZ = *((float *)(buf.data() + 4 * 5)) * GEE; + break; + + default: + break; + } + channelCounter--; + buf.remove(0, 36); + } while (channelCounter); + + + /////// + // Output formatting + /////// + Output2Hardware out; + memset(&out, 0, sizeof(Output2Hardware)); + + // Update GPS Position objects + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude_msl; + out.agl = altitude_agl; + out.groundspeed = groundspeed_ktgs * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] + + out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] + + // Update BaroSensor object + out.temperature = temperature; + out.pressure = pressure; + + // Update attState object + out.roll = roll; // roll; + out.pitch = pitch; // pitch + out.heading = heading; // yaw + + + out.dstN = dstY; + out.dstE = dstX; + out.dstD = -dstZ; + + // Update VelocityState.{North,East,Down} + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; + + // Update gyroscope sensor data - convert from rad/s to deg/s + out.rollRate = rollRate_rad * (180.0 / M_PI); + out.pitchRate = pitchRate_rad * (180.0 / M_PI); + out.yawRate = yawRate_rad * (180.0 / M_PI); + + // Update accelerometer sensor data + out.accX = accX; + out.accY = accY; + out.accZ = -accZ; + + updateUAVOs(out); + } + // issue manual update + // attState->updated(); + // altState->updated(); + // posState->updated(); +} + + +void TraceBuf10(const char *buf, int len) +{ + QString str; + bool reminder = true; + + for (int i = 0; i < len; i++) { + if (!(i % 16)) { + if (i > 0) { + qDebug() << str; + str.clear(); + reminder = false; + } + reminder = true; + } + str += QString(" 0x%1").arg((quint8)buf[i], 2, 16, QLatin1Char('0')); + } + + if (reminder) { + qDebug() << str; + } +} diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h new file mode 100644 index 000000000..7a7632296 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h @@ -0,0 +1,95 @@ +/** + ****************************************************************************** + * + * @file xplanesimulator10.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * 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 XPLANESIMULATOR10_H +#define XPLANESIMULATOR10_H + +#include +#include + +class XplaneSimulator10 : public Simulator { + Q_OBJECT +public: + XplaneSimulator10(const SimulatorSettings & params); + ~XplaneSimulator10(); + bool setupProcess(); + + void setupUdpPorts(const QString & host, int inPort, int outPort); + +private slots: + void transmitUpdate(); + +private: + enum XplaneOutputData // ***WARNING***: Elements in this enum are in a precise order, do + { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml (outdated version 9 info) + // These fields have been updated for X-Plane version 10.x + /* 0 */ FramRate, + /* 1 */ Times, + /* 2 */ SimStats, + /* 3 */ Speed, + /* 4 */ Gload, + /* 5 */ AtmosphereWeather, + /* 6 */ AtmosphereAircraft, + /* 7 */ SystemPressures, + /* 8 */ Joystick1, + /* 9 */ Joystick2, + /* 10 */ ArtStab, + /* 11 */ FlightCon, + /* 12 */ WingSweep, + /* 13 */ Trim, + /* 14 */ Brakes, + /* 15 */ AngularMoments, + /* 16 */ AngularVelocities, + /* 17 */ PitchRollHeading, + /* 18 */ AoA, + /* 19 */ MagCompass, + /* 20 */ LatitudeLongitude, + /* 21 */ LocVelDistTraveled, + /* 22 */ AllPlanesLat, + /* 23 */ AllPlanesLon, + /* 24 */ AllPlanesAlt, + /* 25 */ ThrottleCommand + /* .. */ + + }; + + void processUpdate(const QByteArray & data); +}; + +class XplaneSimulatorCreator10 : public SimulatorCreator { +public: + XplaneSimulatorCreator10(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} + + Simulator *createSimulator(const SimulatorSettings & params) + { + return new XplaneSimulator10(params); + } +}; + +#endif // XPLANESIMULATOR10_H diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.cpp similarity index 92% rename from ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp rename to ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.cpp index fcc8b492f..78852b306 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.cpp @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file xplanesimulator.cpp + * @file xplanesimulator9.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief * @see The GNU Public License (GPL) Version 3 @@ -56,25 +56,25 @@ * */ -#include "xplanesimulator.h" +#include "xplanesimulator9.h" #include "extensionsystem/pluginmanager.h" #include #include #include -void TraceBuf(const char *buf, int len); +void TraceBuf9(const char *buf, int len); -XplaneSimulator::XplaneSimulator(const SimulatorSettings & params) : +XplaneSimulator9::XplaneSimulator9(const SimulatorSettings & params) : Simulator(params) { resetInitialHomePosition(); } -XplaneSimulator::~XplaneSimulator() +XplaneSimulator9::~XplaneSimulator9() {} -void XplaneSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) +void XplaneSimulator9::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); @@ -83,7 +83,7 @@ void XplaneSimulator::setupUdpPorts(const QString & host, int inPort, int outPor resetInitialHomePosition(); } -bool XplaneSimulator::setupProcess() +bool XplaneSimulator9::setupProcess() { emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") + "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); @@ -94,7 +94,7 @@ bool XplaneSimulator::setupProcess() /** * update data in X-Plane simulator */ -void XplaneSimulator::transmitUpdate() +void XplaneSimulator9::transmitUpdate() { if (settings.manualControlEnabled) { // Read ActuatorDesired from autopilot @@ -142,7 +142,7 @@ void XplaneSimulator::transmitUpdate() buf.append(reinterpret_cast(&none), sizeof(none)); buf.append(reinterpret_cast(&none), sizeof(none)); buf.append(reinterpret_cast(&none), sizeof(none)); -// TraceBuf(buf.data(),41); +// TraceBuf9(buf.data(),41); if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); @@ -186,7 +186,7 @@ void XplaneSimulator::transmitUpdate() /** * process data string from X-Plane simulator */ -void XplaneSimulator::processUpdate(const QByteArray & dataBuf) +void XplaneSimulator9::processUpdate(const QByteArray & dataBuf) { float altitude_msl = 0; float altitude_agl = 0; @@ -226,36 +226,36 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) int channelCounter = dataBuf.size() / 36; do { switch (buf[0]) { // switch by id - case XplaneSimulator::LatitudeLongitudeAltitude: + case XplaneSimulator9::LatitudeLongitudeAltitude: latitude = *((float *)(buf.data() + 4 * 1)); longitude = *((float *)(buf.data() + 4 * 2)); altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M; altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M; break; - case XplaneSimulator::Speed: + case XplaneSimulator9::Speed: airspeed_keas = *((float *)(buf.data() + 4 * 2)); groundspeed_ktgs = *((float *)(buf.data() + 4 * 4)); break; - case XplaneSimulator::PitchRollHeading: + case XplaneSimulator9::PitchRollHeading: pitch = *((float *)(buf.data() + 4 * 1)); roll = *((float *)(buf.data() + 4 * 2)); heading = *((float *)(buf.data() + 4 * 3)); break; /* - case XplaneSimulator::SystemPressures: + case XplaneSimulator9::SystemPressures: pressure = *((float*)(buf.data()+4*1)); break; */ - case XplaneSimulator::AtmosphereWeather: + case XplaneSimulator9::AtmosphereWeather: pressure = *((float *)(buf.data() + 4 * 1)) * INHG2KPA; temperature = *((float *)(buf.data() + 4 * 2)); break; - case XplaneSimulator::LocVelDistTraveled: + case XplaneSimulator9::LocVelDistTraveled: dstX = *((float *)(buf.data() + 4 * 1)); dstY = -*((float *)(buf.data() + 4 * 3)); dstZ = *((float *)(buf.data() + 4 * 2)); @@ -264,13 +264,13 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) velZ = *((float *)(buf.data() + 4 * 5)); break; - case XplaneSimulator::AngularVelocities: // In [rad/s] + case XplaneSimulator9::AngularVelocities: // In [rad/s] pitchRate_rad = *((float *)(buf.data() + 4 * 1)); rollRate_rad = *((float *)(buf.data() + 4 * 2)); yawRate_rad = *((float *)(buf.data() + 4 * 3)); break; - case XplaneSimulator::Gload: + case XplaneSimulator9::Gload: accX = *((float *)(buf.data() + 4 * 6)) * GEE; accY = *((float *)(buf.data() + 4 * 7)) * GEE; accZ = *((float *)(buf.data() + 4 * 5)) * GEE; @@ -337,7 +337,7 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) } -void TraceBuf(const char *buf, int len) +void TraceBuf9(const char *buf, int len) { QString str; bool reminder = true; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h similarity index 83% rename from ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h rename to ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h index b1c33f36f..fa7925e3d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file xplanesimulator.h + * @file xplanesimulator9.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief * @see The GNU Public License (GPL) Version 3 @@ -25,17 +25,17 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef XPLANESIMULATOR_H -#define XPLANESIMULATOR_H +#ifndef XPLANESIMULATOR9_H +#define XPLANESIMULATOR9_H #include #include -class XplaneSimulator : public Simulator { +class XplaneSimulator9 : public Simulator { Q_OBJECT public: - XplaneSimulator(const SimulatorSettings & params); - ~XplaneSimulator(); + XplaneSimulator9(const SimulatorSettings & params); + ~XplaneSimulator9(); bool setupProcess(); void setupUdpPorts(const QString & host, int inPort, int outPort); @@ -73,16 +73,16 @@ private: void processUpdate(const QByteArray & data); }; -class XplaneSimulatorCreator : public SimulatorCreator { +class XplaneSimulatorCreator9 : public SimulatorCreator { public: - XplaneSimulatorCreator(const QString & classId, const QString & description) + XplaneSimulatorCreator9(const QString & classId, const QString & description) : SimulatorCreator(classId, description) {} Simulator *createSimulator(const SimulatorSettings & params) { - return new XplaneSimulator(params); + return new XplaneSimulator9(params); } }; -#endif // XPLANESIMULATOR_H +#endif // XPLANESIMULATOR9_H diff --git a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h index eda27387f..ac806e33c 100644 --- a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h +++ b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h @@ -135,10 +135,9 @@ public: }; enum USBConstants { - idVendor_OpenPilot = 0x20a0, - idProduct_OpenPilot = 0x415a, - idProduct_CopterControl = 0x415b, - idProduct_OPLinkMini = 0x415c + idVendor_OpenPilot = 0x20a0, + idProduct_OpenPilot = 0x415a, + idProduct_OPLinkMini = 0x415c }; static USBMonitor *instance(); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 3a122afb9..98d0a267a 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -2320,16 +2320,38 @@ void OPMapGadgetWidget::on_tbFind_clicked() { QPalette pal = m_widget->leFind->palette(); - int result = m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); + QString status = m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); - if (result == core::GeoCoderStatusCode::G_GEO_SUCCESS) { + if (status == "OK") { pal.setColor(m_widget->leFind->backgroundRole(), Qt::green); m_widget->leFind->setPalette(pal); m_map->SetZoom(12); - } else { + } else if (status == "ZERO_RESULTS") { pal.setColor(m_widget->leFind->backgroundRole(), Qt::red); m_widget->leFind->setPalette(pal); + qDebug() << "No results"; + } else if (status == "OVER_QUERY_LIMIT") { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::yellow); + m_widget->leFind->setPalette(pal); + qDebug() << "You are over quota on queries"; + } else if (status == "REQUEST_DENIED") { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::darkRed); + m_widget->leFind->setPalette(pal); + qDebug() << "Request was denied"; + } else if (status == "INVALID_REQUEST") { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::darkYellow); + m_widget->leFind->setPalette(pal); + qDebug() << "Invalid request, missing address, lat long or location"; + } else if (status == "UNKNOWN_ERROR") { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::darkYellow); + m_widget->leFind->setPalette(pal); + qDebug() << "Some sort of server error."; + } else { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::gray); + m_widget->leFind->setPalette(pal); + qDebug() << "Some sort of code error!"; } + } void OPMapGadgetWidget::onHomeDoubleClick(HomeItem *) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index adea4b89f..7acb7721c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -81,10 +81,6 @@ void ConnectionDiagram::setupGraphicsScene() QList elementsToShow; switch (m_configSource->getControllerType()) { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - elementsToShow << "controller-cc"; - break; case VehicleConfigurationSource::CONTROLLER_REVO: elementsToShow << "controller-revo"; break; @@ -164,14 +160,6 @@ void ConnectionDiagram::setupGraphicsScene() QString prefix = ""; QString suffix = ""; switch (m_configSource->getControllerType()) { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - prefix = "cc-"; - if (m_configSource->getEscType() == VehicleConfigurationSource::ESC_ONESHOT || - m_configSource->getEscType() == VehicleConfigurationSource::ESC_SYNCHED) { - suffix = "-oneshot"; - } - break; case VehicleConfigurationSource::CONTROLLER_REVO: prefix = "revo-"; break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp index 6384ef239..4424d811d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -80,9 +80,6 @@ bool ControllerPage::isComplete() const bool ControllerPage::validatePage() { getWizard()->setControllerType((SetupWizard::CONTROLLER_TYPE)ui->boardTypeCombo->itemData(ui->boardTypeCombo->currentIndex()).toInt()); - if (getWizard()->getControllerType() == SetupWizard::CONTROLLER_CC || getWizard()->getControllerType() == SetupWizard::CONTROLLER_CC3D) { - getWizard()->setGpsType(SetupWizard::GPS_DISABLED); - } return true; } @@ -101,12 +98,6 @@ SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() case 0x0301: return SetupWizard::CONTROLLER_OPLINK; - case 0x0401: - return SetupWizard::CONTROLLER_CC; - - case 0x0402: - return SetupWizard::CONTROLLER_CC3D; - case 0x0903: return SetupWizard::CONTROLLER_REVO; @@ -129,8 +120,6 @@ void ControllerPage::setupBoardTypes() QVariant v(0); ui->boardTypeCombo->addItem(tr(""), SetupWizard::CONTROLLER_UNKNOWN); - ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl"), SetupWizard::CONTROLLER_CC); - ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl 3D"), SetupWizard::CONTROLLER_CC3D); ui->boardTypeCombo->addItem(tr("OpenPilot Revolution"), SetupWizard::CONTROLLER_REVO); ui->boardTypeCombo->addItem(tr("OpenPilot OPLink Radio Modem"), SetupWizard::CONTROLLER_OPLINK); ui->boardTypeCombo->addItem(tr("OpenPilot DiscoveryF4"), SetupWizard::CONTROLLER_DISCOVERYF4); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp index 92bd64206..d56a2ec1b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp @@ -73,29 +73,5 @@ void EscPage::initializePage() bool EscPage::isSynchOrOneShotAvailable() { - bool available = true; - - switch (getWizard()->getControllerType()) { - case SetupWizard::CONTROLLER_CC: - case SetupWizard::CONTROLLER_CC3D: - switch (getWizard()->getVehicleType()) { - case SetupWizard::VEHICLE_MULTI: - switch (getWizard()->getVehicleSubType()) { - case SetupWizard::MULTI_ROTOR_TRI_Y: - case SetupWizard::MULTI_ROTOR_QUAD_X: - case SetupWizard::MULTI_ROTOR_QUAD_H: - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - available = getWizard()->getInputType() != SetupWizard::INPUT_PWM; - break; - default: - available = false; - break; - } - break; - default: break; - } - break; - default: break; - } - return available; + return true; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index 849284e68..1fd221706 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -72,27 +72,6 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp HwSettings *hwSettings = HwSettings::GetInstance(uavoManager); HwSettings::DataFields data = hwSettings->getData(); switch (getWizard()->getControllerType()) { - case SetupWizard::CONTROLLER_CC: - case SetupWizard::CONTROLLER_CC3D: - { - switch (selectedType) { - case VehicleConfigurationSource::INPUT_PWM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWMNOONESHOT; - - case VehicleConfigurationSource::INPUT_PPM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPMNOONESHOT; - - case VehicleConfigurationSource::INPUT_SBUS: - return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - - case VehicleConfigurationSource::INPUT_DSM: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM; - - default: return true; - } - break; - } case SetupWizard::CONTROLLER_REVO: case SetupWizard::CONTROLLER_DISCOVERYF4: { diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg index eef9bb92b..e01370ac0 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg +++ b/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg @@ -15,7 +15,7 @@ height="1074.8231" width="1398.5884" version="1.1" - inkscape:version="0.48.5 r10040" + inkscape:version="0.48.4 r9939" sodipodi:docname="connection-diagrams.svg"> Satellite - - - - - + style="display:none"> @@ -30266,7 +29551,7 @@ id="path5636-1" /> @@ -30278,19 +29563,19 @@ id="path5728-7" /> @@ -30363,73 +29648,73 @@ id="path12850-3" /> @@ -30441,7 +29726,7 @@ id="path12876-7" /> @@ -30453,19 +29738,19 @@ id="path12880-3" /> @@ -30538,73 +29823,73 @@ id="path12850_1_" /> @@ -30616,7 +29901,7 @@ id="path12876_1_" /> @@ -30628,19 +29913,19 @@ id="path12880_1_" /> @@ -30654,7 +29939,7 @@ @@ -30681,7 +29966,7 @@ id="path3787-5" /> @@ -30696,7 +29981,7 @@ id="path3793-1" /> @@ -30711,7 +29996,7 @@ id="path3799-5" /> @@ -30726,7 +30011,7 @@ id="path3805-2" /> @@ -30741,7 +30026,7 @@ id="path3811-3" /> @@ -30756,7 +30041,7 @@ id="path3817-6" /> @@ -30771,7 +30056,7 @@ id="path3823-6" /> @@ -30786,7 +30071,7 @@ id="path3829-6" /> @@ -30801,7 +30086,7 @@ id="path3835-7" /> @@ -30816,7 +30101,7 @@ id="path3841-9" /> @@ -30831,7 +30116,7 @@ id="path3847-6" /> @@ -30846,7 +30131,7 @@ id="path3853-7" /> @@ -30861,7 +30146,7 @@ id="path3859-7" /> @@ -30876,7 +30161,7 @@ id="path3865-5" /> @@ -30891,7 +30176,7 @@ id="path3871-3" /> @@ -30901,12 +30186,12 @@ id="g3927"> @@ -30916,12 +30201,12 @@ id="g3933"> @@ -30931,12 +30216,12 @@ id="g3939"> @@ -30946,12 +30231,12 @@ id="g3945"> @@ -30961,12 +30246,12 @@ id="g3951"> @@ -30976,12 +30261,12 @@ id="g3957"> @@ -30991,12 +30276,12 @@ id="g3963"> @@ -31006,12 +30291,12 @@ id="g3969"> @@ -31021,12 +30306,12 @@ id="g3975"> @@ -31036,12 +30321,12 @@ id="g3981"> @@ -31051,12 +30336,12 @@ id="g3987"> @@ -31066,12 +30351,12 @@ id="g3993"> @@ -31081,12 +30366,12 @@ id="g3999"> @@ -31096,12 +30381,12 @@ id="g4005"> @@ -31111,12 +30396,12 @@ id="g4011"> @@ -31126,12 +30411,12 @@ id="g4017"> @@ -31141,12 +30426,12 @@ id="g4025"> @@ -31156,12 +30441,12 @@ id="g4031"> @@ -31171,12 +30456,12 @@ id="g4037"> @@ -31186,12 +30471,12 @@ id="g4043"> @@ -31201,12 +30486,12 @@ id="g4049"> @@ -31216,12 +30501,12 @@ id="g4055"> @@ -31231,12 +30516,12 @@ id="g4061"> @@ -31251,7 +30536,7 @@ id="path4069-6" /> @@ -31261,12 +30546,12 @@ id="g4073"> @@ -31276,12 +30561,12 @@ id="g4079"> @@ -31291,12 +30576,12 @@ id="g4085"> @@ -31306,12 +30591,12 @@ id="g4091"> @@ -31326,7 +30611,7 @@ id="path4099-5" /> @@ -31336,12 +30621,12 @@ id="g4103"> @@ -31351,12 +30636,12 @@ id="g4109"> @@ -31366,12 +30651,12 @@ id="g4115"> @@ -31381,12 +30666,12 @@ id="g4123"> @@ -31396,12 +30681,12 @@ id="g4129"> @@ -31411,12 +30696,12 @@ id="g4135"> @@ -31426,12 +30711,12 @@ id="g4141"> @@ -31441,12 +30726,12 @@ id="g4147"> @@ -31456,12 +30741,12 @@ id="g4153"> @@ -31471,12 +30756,12 @@ id="g4159"> @@ -31486,12 +30771,12 @@ id="g4165"> @@ -31501,12 +30786,12 @@ id="g4171"> @@ -31516,12 +30801,12 @@ id="g4177"> @@ -31531,12 +30816,12 @@ id="g4183"> @@ -31546,12 +30831,12 @@ id="g4189"> @@ -31561,12 +30846,12 @@ id="g4195"> @@ -31576,12 +30861,12 @@ id="g4201"> @@ -31591,12 +30876,12 @@ id="g4207"> @@ -31606,12 +30891,12 @@ id="g4213"> @@ -31689,7 +30974,7 @@ @@ -32641,11 +31926,11 @@ + d="m 110.126,10.926 0.563,1.734 c 3.82,-1.425 2.62,-7.761 -2.074,-6.405 4.021,-0.446 4.084,4.377 1.511,4.671 z" /> + d="m 108.854,7.167 -0.563,-1.734 c -3.82,1.425 -2.62,7.761 2.075,6.405 -4.021,0.446 -4.084,-4.377 -1.512,-4.671 z" /> + d="m 98.619,13.405 h -1.59 L 95.548,10.35 H 95.39 v 2.083 H 93.94 V 5.672 c 0.381,-0.009 1.283,-0.148 1.711,-0.148 0.437,0 0.949,0.019 1.396,0.25 0.734,0.372 1.134,1.154 1.087,2.558 -0.093,1.144 -0.502,1.692 -1.125,1.916 l 1.61,3.157 z m -1.936,-5.39 c 0,-0.576 -0.205,-1.199 -0.819,-1.199 -0.158,0 -0.333,0.018 -0.474,0.046 v 2.38 c 0.892,0.336 1.293,-0.389 1.293,-1.227 z" /> + d="M 98.329,12.433 V 5.672 h 3.05 v 1.274 h -1.562 v 1.479 h 1.488 v 1.209 h -1.488 v 1.479 h 1.562 v 1.32 h -3.05 z" /> + d="m 103.88,10.359 0.707,-4.687 h 1.553 l -1.404,6.761 h -1.85 l -1.414,-6.761 h 1.562 l 0.708,4.687 0.074,0.567 0.064,-0.567 z" /> + d="M 113.285,12.433 V 5.672 h 1.469 v 5.44 h 1.646 v 1.32 h -3.115 z" /> + d="M 116.558,10.573 V 5.672 h 1.487 v 4.817 c 0.009,0.456 0.038,0.818 0.651,0.818 0.613,0 0.642,-0.362 0.651,-0.818 V 5.672 h 1.487 v 4.901 c 0,1.376 -0.763,1.99 -2.139,1.99 -1.356,0 -2.137,-0.614 -2.137,-1.99 z" /> + d="m 121.022,6.946 0.102,-1.274 h 3.952 l 0.121,1.274 h -1.292 v 5.487 H 122.38 V 6.946 h -1.358 z" /> + d="m 125.41,5.672 h 1.517 v 6.761 H 125.41 V 5.672 z" /> + d="m 127.177,9.029 c 0,-1.692 0.633,-3.542 2.604,-3.533 1.962,-0.009 2.595,1.841 2.586,3.533 0,1.711 -0.595,3.543 -2.586,3.534 -1.999,0.009 -2.584,-1.823 -2.604,-3.534 z m 1.563,0.019 c 0,0.93 0.186,2.269 1.041,2.306 0.847,-0.038 1.032,-1.376 1.032,-2.306 0,-0.921 -0.186,-2.111 -1.032,-2.167 -0.856,0.056 -1.041,1.246 -1.041,2.167 z" /> + d="M 132.598,12.433 V 5.672 h 1.256 l 1.423,3.32 v -3.32 h 1.433 v 6.761 h -1.19 l -1.469,-3.311 v 3.311 h -1.453 z" /> + d="m 35.028,179.497 c 0,1.104 -0.896,2 -2,2 H 6.084 c -1.104,0 -2,-0.896 -2,-2 l 2,-1.341 -2,-2.125 v -31 l 2,-2.375 -2,-1.288 c 0,-1.104 0.896,-2 2,-2 h 26.945 c 1.104,0 2,0.896 2,2 v 38.129 z" /> + points="186.083,4.74 191.036,13.319 186.194,11.031 181.129,13.319 " /> + points="36.083,4.74 41.036,13.319 36.194,11.031 31.129,13.319 " /> @@ -32981,2619 +32266,27 @@ transform="translate(-721.32539,-464.51938)"> - - - - + d="m 1286.1956,653.36166 c 0,0.42069 -0.1679,0.77058 -0.5036,1.04968 -0.3156,0.27102 -0.6877,0.40653 -1.1165,0.40652 -0.4328,0 -0.8009,-0.13348 -1.1043,-0.40045 l -2.0144,5.59425 c -0.1901,0.53799 -0.5198,0.80698 -0.989,0.80698 -0.631,0 -1.0052,-0.27304 -1.1225,-0.81911 l -1.0072,-4.77514 -1.3652,4.04097 c -0.1415,0.42068 -0.4126,0.75642 -0.813,1.0072 -0.3762,0.23866 -0.7928,0.35799 -1.2499,0.35799 l -2.8821,0 c 0.178,-0.44495 0.3297,-0.75844 0.4551,-0.94047 0.2386,-0.35191 0.5177,-0.52787 0.8373,-0.52787 0.1941,0 0.4874,0.0425 0.8798,0.12742 0.3964,0.0809 0.6957,0.12135 0.898,0.12135 0.4247,0 0.726,-0.26091 0.904,-0.78271 l 1.8688,-5.4183 c 0.1578,-0.46921 0.2933,-0.77056 0.4066,-0.90406 0.1901,-0.24269 0.4894,-0.36404 0.8979,-0.36405 0.6108,0 1.0032,0.4308 1.1771,1.29238 l 0.8313,4.15019 1.256,-3.52524 c 0.4651,-1.30248 1.173,-1.95373 2.1236,-1.95374 0.4288,0 0.8029,0.1335 1.1225,0.40046 0.3398,0.28316 0.5097,0.63508 0.5097,1.05575" /> + d="m 1291.7352,659.65974 c -0.1942,0.64721 -0.6897,0.97081 -1.4865,0.97081 l -2.2996,0 c -0.6553,0 -0.983,-0.21034 -0.983,-0.63102 -0.4409,0.50158 -0.9829,0.75237 -1.6261,0.75237 -0.4732,0 -0.8575,-0.14562 -1.1528,-0.43686 -0.2953,-0.29124 -0.4429,-0.67147 -0.4429,-1.1407 0,-0.8454 0.3236,-1.51283 0.9708,-2.00228 0.5865,-0.44494 1.3126,-0.66742 2.1782,-0.66742 l 3.4646,0 -1.1407,3.1551 2.518,0 m -3.9135,-2.1843 -1.341,0 -0.4247,1.41373 c -0.024,0.089 -0.036,0.16989 -0.036,0.2427 0,0.35192 0.3438,0.52787 1.0315,0.52787 l 0.7706,-2.1843" /> + d="m 1300.0477,659.65974 c -0.1052,0.35597 -0.3378,0.61889 -0.6978,0.78878 -0.2548,0.12135 -0.5278,0.18203 -0.8191,0.18203 l -2.4209,0 c -0.5542,0 -0.8313,-0.18203 -0.8313,-0.54608 0,-0.12135 0.028,-0.26292 0.085,-0.42473 l 0.4611,-1.30451 -2.0872,1.76565 c -0.4895,0.33978 -0.8919,0.50967 -1.2074,0.50967 l -1.1165,0 1.4502,-4.12591 2.1903,0 -0.6856,1.9416 1.8506,-1.51688 c 0.5016,-0.40854 1.0456,-0.60472 1.6322,-0.58855 0.3964,0.0162 0.5946,0.22653 0.5946,0.63102 0,0.14967 -0.032,0.31754 -0.097,0.50361 l -0.7827,2.1843 2.4816,0" /> + d="m 1310.314,656.50464 c -0.1619,0.6472 -0.6473,0.9708 -1.4562,0.9708 l -3.7983,0 c 0.1335,0.089 0.2427,0.25281 0.3276,0.49147 0.085,0.23461 0.1194,0.43889 0.1032,0.61282 -0.057,0.67552 -0.354,1.20541 -0.8919,1.58969 -0.5138,0.36809 -1.1812,0.55416 -2.0023,0.55821 -0.9547,0.008 -1.6888,-0.18001 -2.2025,-0.56428 -0.4571,-0.33978 -0.6857,-0.78675 -0.6857,-1.34092 0,-0.10921 0.01,-0.21843 0.03,-0.32765 0.1051,-0.61079 0.4085,-1.09013 0.9101,-1.438 0.5339,-0.36809 1.2459,-0.55214 2.1358,-0.55214 l 7.5298,0 m -6.6136,0.9708 -1.3895,0 -0.5461,1.55329 c -0.028,0.0809 -0.043,0.15775 -0.043,0.23056 0,0.34787 0.3236,0.52181 0.9709,0.52181 0.077,0 0.1435,-0.002 0.2002,-0.006 l 0.807,-2.29959" /> @@ -37012,19 +33705,19 @@ style="font-size:12px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Arial;-inkscape-font-specification:Arial Bold" id="text15590"> @@ -37378,7 +34071,7 @@ style="fill:none;stroke:#000000;stroke-width:0.99212599px;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none" /> + d="m 386.44699,686.89099 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51878,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51878 1.23122,-2.75 2.75,-2.75 1.51878,0 2.75,1.23122 2.75,2.75 z" /> + d="m 411.435,717.672 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51879,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51879 1.23121,-2.75 2.75,-2.75 1.51878,0 2.75,1.23121 2.75,2.75 z" /> + d="m 216.804,613.62 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51878,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51879 1.23122,-2.75 2.75,-2.75 1.51878,0 2.75,1.23121 2.75,2.75 z" /> + d="m 216.804,613.62 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51878,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51879 1.23122,-2.75 2.75,-2.75 1.51878,0 2.75,1.23121 2.75,2.75 z" /> + d="m 586.448,613.99402 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51879,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51878 1.23121,-2.75 2.75,-2.75 1.51878,0 2.75,1.23122 2.75,2.75 z" /> + d="m 386.44699,686.89099 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51878,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51878 1.23122,-2.75 2.75,-2.75 1.51878,0 2.75,1.23122 2.75,2.75 z" /> + d="m 411.435,717.672 c 0,1.51878 -1.23122,2.75 -2.75,2.75 -1.51879,0 -2.75,-1.23122 -2.75,-2.75 0,-1.51879 1.23121,-2.75 2.75,-2.75 1.51878,0 2.75,1.23121 2.75,2.75 z" /> ").append(tr("Controller type: ")).append(""); switch (getControllerType()) { - case CONTROLLER_CC: - summary.append(tr("OpenPilot CopterControl")); - break; - case CONTROLLER_CC3D: - summary.append(tr("OpenPilot CopterControl 3D")); - break; case CONTROLLER_REVO: summary.append(tr("OpenPilot Revolution")); break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index a14970afe..be8604362 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -100,15 +100,7 @@ bool VehicleConfigurationHelper::setupHardwareSettings(bool save) bool VehicleConfigurationHelper::isApplicable(UAVObject *dataObj) { - switch (m_configSource->getControllerType()) { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - if (dataObj->getName() == "EKFConfiguration") { - return false; - } - default: - return true; - } + return true; } void VehicleConfigurationHelper::addModifiedObject(UAVDataObject *object, QString description) @@ -136,39 +128,6 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() data.OptionalModules[HwSettings::OPTIONALMODULES_AIRSPEED] = 0; switch (m_configSource->getControllerType()) { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - // Reset all ports - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLEDONESHOT; - - // Default mainport to be active telemetry link - data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; - - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; - switch (m_configSource->getInputType()) { - case VehicleConfigurationSource::INPUT_PWM: - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWMNOONESHOT; - break; - case VehicleConfigurationSource::INPUT_PPM: - if (m_configSource->getEscType() == VehicleConfigurationSource::ESC_ONESHOT || - m_configSource->getEscType() == VehicleConfigurationSource::ESC_SYNCHED) { - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM_PIN8ONESHOT; - } else { - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPMNOONESHOT; - } - break; - case VehicleConfigurationSource::INPUT_SBUS: - // We have to set teletry on flexport since s.bus needs the mainport. - data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; - break; - case VehicleConfigurationSource::INPUT_DSM: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM; - break; - default: - break; - } - break; case VehicleConfigurationSource::CONTROLLER_REVO: case VehicleConfigurationSource::CONTROLLER_NANO: case VehicleConfigurationSource::CONTROLLER_DISCOVERYF4: @@ -430,10 +389,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() // Servo always on channel 4 data.BankUpdateFreq[0] = escFrequence; data.BankMode[0] = bankMode; - if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_CC || - m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_CC3D) { - data.BankUpdateFreq[1] = servoFrequence; - } else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { data.BankUpdateFreq[1] = escFrequence; data.BankMode[1] = bankMode; data.BankUpdateFreq[2] = servoFrequence; @@ -625,20 +581,6 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings")); switch (m_configSource->getControllerType()) { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - { - AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); - Q_ASSERT(copterControlCalibration); - AttitudeSettings::DataFields data = copterControlCalibration->getData(); - - data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU; - data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; - - copterControlCalibration->setData(data); - addModifiedObject(copterControlCalibration, tr("Writing board settings")); - break; - } case VehicleConfigurationSource::CONTROLLER_REVO: { RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index b0b2b09d0..cf7127bdb 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -57,7 +57,7 @@ class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4 }; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4 }; enum VEHICLE_TYPE { VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE }; enum VEHICLE_SUB_TYPE { MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, MULTI_ROTOR_QUAD_H, MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index ae575125d..dd6334230 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -126,6 +126,7 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/hwsettings.xml \ $${UAVOBJ_XML_DIR}/gcsreceiver.xml \ $${UAVOBJ_XML_DIR}/receiveractivity.xml \ + $${UAVOBJ_XML_DIR}/receiverstatus.xml \ $${UAVOBJ_XML_DIR}/attitudesettings.xml \ $${UAVOBJ_XML_DIR}/txpidsettings.xml \ $${UAVOBJ_XML_DIR}/cameradesired.xml \ diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index fc3e0c3cd..62b4ea01c 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -14,51 +14,23 @@ public: static QString idToBoardName(int id) { switch (id) { - case 0x0101: - // MB - return QString("OpenPilot MainBoard"); - - break; - case 0x0201: - // INS - return QString("OpenPilot INS"); - - break; case 0x0301: // OPLink Mini return QString("OPLink"); - break; - case 0x0401: - // Coptercontrol - return QString("CopterControl"); - - break; - case 0x0402: - // Coptercontrol 3D - // It would be nice to say CC3D here but since currently we use string comparisons - // for firmware compatibility and the filename path that would break - return QString("CopterControl"); - - break; case 0x0901: // Revolution return QString("Revolution"); - break; case 0x0903: // Revo Mini return QString("Revolution"); - break; case 0x0904: return QString("DiscoveryF4"); - break; default: return QString(""); - - break; } } deviceDescriptorStruct() {} diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index 451f63a12..fab29c935 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -78,6 +78,11 @@ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; + // Exclude CC/CC3D + if ((id == 0x0401) || (id == 0x0402)) { + return; + } + myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: @@ -86,22 +91,10 @@ void DeviceWidget::populate() myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16)); switch (id) { - case 0x0101: - case 0x0201: - devicePic.load(""); - break; case 0x0301: devicePic.load(":/uploader/images/gcs-board-oplink.png"); break; - case 0x0401: - devicePic.load(":/uploader/images/gcs-board-cc.png"); - break; - case 0x0402: - devicePic.load(":/uploader/images/gcs-board-cc3d.png"); - break; case 0x0903: - devicePic.load(":/uploader/images/gcs-board-revo.png"); - break; case 0x0904: devicePic.load(":/uploader/images/gcs-board-revo.png"); break; @@ -361,8 +354,7 @@ void DeviceWidget::uploadFirmware() // - Check whether board type matches firmware: int board = m_dfu->devices[deviceID].ID; int firmwareBoard = ((desc.at(12) & 0xff) << 8) + (desc.at(13) & 0xff); - if ((board == 0x401 && firmwareBoard == 0x402) || - (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware + if ((board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware (board == 0x902 && firmwareBoard == 0x903)) { // RevoMini1 supporetd by RevoMini2 firmware // These firmwares are designed to be backwards compatible } else if (firmwareBoard != board) { diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg deleted file mode 100644 index adc9a3d05..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg +++ /dev/null @@ -1,2466 +0,0 @@ - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg deleted file mode 100644 index 3b6a15ebb..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg +++ /dev/null @@ -1,2948 +0,0 @@ - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg deleted file mode 100644 index be0166618..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - image/svg+xml - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg deleted file mode 100644 index d18741f4a..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg +++ /dev/null @@ -1,2646 +0,0 @@ - - - -image/svg+xml \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg deleted file mode 100644 index 90121943d..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg +++ /dev/null @@ -1,2389 +0,0 @@ - - - -image/svg+xmlCC3D - \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png deleted file mode 100644 index ee6affc3c..000000000 Binary files a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png and /dev/null differ diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png deleted file mode 100644 index 24326c952..000000000 Binary files a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png and /dev/null differ diff --git a/ground/openpilotgcs/src/plugins/uploader/images/pipx.png b/ground/openpilotgcs/src/plugins/uploader/images/pipx.png deleted file mode 100644 index 47c8bb4e1..000000000 Binary files a/ground/openpilotgcs/src/plugins/uploader/images/pipx.png and /dev/null differ diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 58401b8a0..b516149f7 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -1074,7 +1074,6 @@ int DFUObject::receiveData(void *data, int size) #define BOARD_ID_MB 1 #define BOARD_ID_INS 2 #define BOARD_ID_PIP 3 -#define BOARD_ID_CC 4 #define BOARD_ID_REVO 9 /** @@ -1098,9 +1097,6 @@ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum) case BOARD_ID_PIP: // PIP RF Modem brdType = eBoardPip; break; - case BOARD_ID_CC: // CopterControl family - brdType = eBoardCC; - break; case BOARD_ID_REVO: // Revo board brdType = eBoardRevo; break; diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h index 346582872..4409bc117 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h @@ -92,7 +92,6 @@ enum eBoardType { eBoardMainbrd = 1, eBoardINS, eBoardPip = 3, - eBoardCC = 4, eBoardRevo = 9, }; diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 26116eae2..1db201e33 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -72,19 +72,9 @@ void RunningDeviceWidget::populate() myDevice->devicePicture->scene()->clear(); switch (id) { - case 0x0101: - case 0x0201: - devicePic.load(""); - break; case 0x0301: devicePic.load(":/uploader/images/gcs-board-oplink.png"); break; - case 0x0401: - devicePic.load(":/uploader/images/gcs-board-cc.png"); - break; - case 0x0402: - devicePic.load(":/uploader/images/gcs-board-cc3d.png"); - break; case 0x0903: devicePic.load(":/uploader/images/gcs-board-revo.png"); break; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc index d2cd82ca6..938a7c806 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc @@ -5,17 +5,9 @@ images/process-stop.svg images/dialog-apply.svg images/gtk-info.svg - images/deviceID-0401.svg - images/deviceID-0301.svg - images/deviceID-0201.svg - images/deviceID-0101.svg images/application-certificate.svg images/warning.svg images/error.svg - images/deviceID-0402.svg - images/gcs-board-cc.png - images/gcs-board-cc3d.png - images/pipx.png images/gcs-board-oplink.png images/gcs-board-revo.png diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 35023fd78..3c2494d33 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -752,10 +752,6 @@ bool UploaderGadgetWidget::autoUpdate(bool erase) case 0x301: filename = "fw_oplinkmini"; break; - case 0x401: - case 0x402: - filename = "fw_coptercontrol"; - break; case 0x501: filename = "fw_osd"; break; @@ -814,7 +810,6 @@ bool UploaderGadgetWidget::autoUpdate(bool erase) commonSystemBoot(false, erase); // Wait for board to connect to GCS again after boot and erase - // For older board like CC3D this can take some time // Theres a special case with OPLink if (!telemetryManager->isConnected() && !m_oplinkwatchdog.isConnected()) { progressUpdate(erase ? BOOTING_AND_ERASING : BOOTING, QVariant()); diff --git a/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp b/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp index 10a1cb294..5cebee5af 100644 --- a/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp +++ b/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp @@ -178,18 +178,11 @@ void UsageTrackerPlugin::collectUsageParameters(QMap ¶mete parameters["conf_receiver"] = getUAVFieldValue(objManager, "ManualControlSettings", "ChannelGroups", 0); parameters["conf_vehicle"] = getUAVFieldValue(objManager, "SystemSettings", "AirframeType"); - if ((boardModel & 0xff00) == 0x0400) { - // CopterControl family - parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "CC_RcvrPort"); - parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "CC_MainPort"); - parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "CC_FlexiPort"); - } else if ((boardModel & 0xff00) == 0x0900) { - // Revolution family - parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "RM_RcvrPort"); - parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "RM_MainPort"); - parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "RM_FlexiPort"); - parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm"); - } + // Revolution family + parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "RM_RcvrPort"); + parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "RM_MainPort"); + parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "RM_FlexiPort"); + parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm"); parameters["conf_uport"] = getUAVFieldValue(objManager, "HwSettings", "USB_HIDPort"); parameters["conf_vport"] = getUAVFieldValue(objManager, "HwSettings", "USB_VCPPort"); diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 744827d2b..d197221a8 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -33,6 +33,10 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc OPUAVOBJINC = $(OPUAVOBJ)/inc OPUAVTALKINC = $(OPUAVTALK)/inc +## PID +PIDLIB =$(FLIGHTLIB)/pid +PIDLIBINC =$(FLIGHTLIB)/pid + ## Math MATHLIB = $(FLIGHTLIB)/math MATHLIBINC = $(FLIGHTLIB)/math @@ -88,7 +92,10 @@ SRC += $(PIOSCOMMON)/pios_sensors.c SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(MATHLIB)/sin_lookup.c + +## PID library functions SRC += $(MATHLIB)/pid.c +CPPSRC += $(PIDLIB)/pidcontroldown.cpp ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c @@ -167,6 +174,7 @@ EXTRAINCDIRS += $(FLIGHTLIBINC) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(OPSYSTEMINC) EXTRAINCDIRS += $(MATHLIBINC) +EXTRAINCDIRS += $(PIDLIBINC) EXTRAINCDIRS += $(OPUAVOBJINC) EXTRAINCDIRS += $(OPUAVTALKINC) EXTRAINCDIRS += $(OPUAVSYNTHDIR) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index aadf90e8f..e85ff09e6 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,14 +1,15 @@ - Settings for the @ref AltitudeHold module - - + Settings for the @ref AltitudeHold module - - - - - - + + + + + + + + + diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml index f298a45ab..0918785c5 100644 --- a/shared/uavobjectdefinition/altitudeholdstatus.xml +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -2,6 +2,8 @@ Status Data from Altitude Hold Control Loops + + diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index ef69c8cb8..129faa688 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 6a95382d4..d2ddbab34 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -12,7 +12,7 @@ - + diff --git a/shared/uavobjectdefinition/receiverstatus.xml b/shared/uavobjectdefinition/receiverstatus.xml new file mode 100644 index 000000000..5a0415be2 --- /dev/null +++ b/shared/uavobjectdefinition/receiverstatus.xml @@ -0,0 +1,11 @@ + + + Receiver status. + + + + + + + + diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 28035452b..6d59ae5a9 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -20,7 +20,7 @@ Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp, Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp, Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo, - GyroTau,AcroPlusFactor" + GyroTau,AcroPlusFactor,Altitude Pos.Kp,Altitude Velocity.Kp,Altitude Velocity.Ki,Altitude Velocity.Kd,Altitude Velocity.Beta" defaultvalue="Disabled"/> diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index c98ad4254..4f6758035 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -7,8 +7,8 @@ - - + + @@ -21,10 +21,10 @@ - + - - + +