mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Merged in f5soh/librepilot/shared/LP-72_Sparky2_support_rebased (pull request #251)
shared/LP-72_sparky2_support_rebased
This commit is contained in:
commit
343ae71ce1
2
Makefile
2
Makefile
@ -304,7 +304,7 @@ uploader_clean:
|
||||
#
|
||||
##############################
|
||||
# Firmware files to package
|
||||
PACKAGE_FW_TARGETS := fw_coptercontrol fw_oplinkmini fw_revolution fw_osd fw_revoproto fw_gpsplatinum fw_revonano
|
||||
PACKAGE_FW_TARGETS := fw_coptercontrol fw_oplinkmini fw_revolution fw_osd fw_revoproto fw_gpsplatinum fw_revonano fw_sparky2
|
||||
|
||||
# Rules to generate GCS resources used to embed firmware binaries into the GCS.
|
||||
# They are used later by the vehicle setup wizard to update board firmware.
|
||||
|
@ -8,7 +8,7 @@ export OPUAVTALK := $(FLIGHT_ROOT_DIR)/uavtalk
|
||||
export FLIGHT_OUT_DIR ?= $(CURDIR)
|
||||
|
||||
# Define supported board lists
|
||||
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum revonano
|
||||
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum revonano sparky2
|
||||
|
||||
# Short names of each board (used to display board name in parallel builds)
|
||||
coptercontrol_short := 'cc '
|
||||
@ -17,6 +17,7 @@ revolution_short := 'revo'
|
||||
osd_short := 'osd '
|
||||
revoproto_short := 'revp'
|
||||
revonano_short := 'revn'
|
||||
sparky2_short := 'spk2'
|
||||
simposix_short := 'posx'
|
||||
discoveryf4bare_short := 'df4b'
|
||||
gpsplatinum_short := 'gps9'
|
||||
|
@ -3,7 +3,7 @@ Signature = "$Windows NT$"
|
||||
Class = Ports
|
||||
ClassGuid = {4D36E978-E325-11CE-BFC1-08002BE10318}
|
||||
Provider = %ProviderName%
|
||||
DriverVer=11/21/2014,3.0.0.0
|
||||
DriverVer=05/25/2016,4.0.0.0
|
||||
CatalogFile.NTx86 = OpenPilot-CDC_x86.cat
|
||||
CatalogFile.NTamd64 = OpenPilot-CDC_amd64.cat
|
||||
|
||||
@ -15,12 +15,14 @@ CatalogFile.NTamd64 = OpenPilot-CDC_amd64.cat
|
||||
%Revolution% = DriverInstall,USB\VID_20A0&PID_415e&MI_00
|
||||
%OPLinkMini% = DriverInstall,USB\VID_20A0&PID_415c&MI_00
|
||||
%OPLink1W% = DriverInstall,USB\VID_20A0&PID_4195&MI_00
|
||||
%Sparky2% = DriverInstall,USB\VID_20A0&PID_41d0&MI_00
|
||||
|
||||
[DeviceList.NTamd64]
|
||||
%CopterControl% = DriverInstall,USB\VID_20A0&PID_415b&MI_00
|
||||
%Revolution% = DriverInstall,USB\VID_20A0&PID_415e&MI_00
|
||||
%OPLinkMini% = DriverInstall,USB\VID_20A0&PID_415c&MI_00
|
||||
%OPLink1W% = DriverInstall,USB\VID_20A0&PID_4195&MI_00
|
||||
%Sparky2% = DriverInstall,USB\VID_20A0&PID_41d0&MI_00
|
||||
|
||||
[DriverInstall]
|
||||
include = mdmcpq.inf
|
||||
@ -41,3 +43,4 @@ CopterControl = "CopterControl Virtual COM Port"
|
||||
Revolution = "Revolution Virtual COM Port"
|
||||
OPLinkMini = "OPLinkMini Virtual COM Port"
|
||||
OPLink1W = "OPLink1W Virtual COM Port"
|
||||
Sparky2 = "Sparky2 Virtual COM Port"
|
||||
|
@ -360,7 +360,7 @@ size_t AlarmString(SystemAlarmsData *alarm, char *buffer, size_t buffer_size, Sy
|
||||
}
|
||||
|
||||
int current_len = strlen(current_msg);
|
||||
|
||||
|
||||
if ((pos + current_len + 1) > buffer_size) {
|
||||
break;
|
||||
}
|
||||
|
@ -655,6 +655,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
|
||||
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
|
||||
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
|
||||
break;
|
||||
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT:
|
||||
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMRCVRPORT;
|
||||
break;
|
||||
case MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS:
|
||||
group = RECEIVERACTIVITY_ACTIVEGROUP_EXBUS;
|
||||
break;
|
||||
|
@ -227,18 +227,18 @@ static const char msp_pidnames[] = "ROLL;"
|
||||
#define MSP_ANALOG_CURRENT (1 << 1)
|
||||
|
||||
struct msp_bridge {
|
||||
uintptr_t com;
|
||||
uintptr_t com;
|
||||
|
||||
uint8_t sensors;
|
||||
uint8_t analog;
|
||||
|
||||
uint8_t sensors;
|
||||
uint8_t analog;
|
||||
|
||||
UAVObjHandle current_pid_bank;
|
||||
|
||||
msp_state state;
|
||||
uint8_t cmd_size;
|
||||
uint8_t cmd_id;
|
||||
uint8_t cmd_i;
|
||||
uint8_t checksum;
|
||||
|
||||
msp_state state;
|
||||
uint8_t cmd_size;
|
||||
uint8_t cmd_id;
|
||||
uint8_t cmd_i;
|
||||
uint8_t checksum;
|
||||
union {
|
||||
uint8_t data[0];
|
||||
// Specific packed data structures go here.
|
||||
@ -489,13 +489,13 @@ static void msp_send_analog(struct msp_bridge *m)
|
||||
data.status.rssi = ((rssi - OPLINK_LOW_RSSI) * 1023) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI);
|
||||
} else {
|
||||
#endif /* PIOS_INCLUDE_OPLINKRCVR */
|
||||
uint8_t quality;
|
||||
ReceiverStatusQualityGet(&quality);
|
||||
uint8_t quality;
|
||||
ReceiverStatusQualityGet(&quality);
|
||||
|
||||
// MSP RSSI's range is 0-1023
|
||||
data.status.rssi = (quality * 1023) / 100;
|
||||
// MSP RSSI's range is 0-1023
|
||||
data.status.rssi = (quality * 1023) / 100;
|
||||
#ifdef PIOS_INCLUDE_OPLINKRCVR
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_OPLINKRCVR */
|
||||
|
||||
if (data.status.rssi > 1023) {
|
||||
@ -674,14 +674,14 @@ static void msp_send_pidnames(struct msp_bridge *m)
|
||||
|
||||
static void pid_native2msp(const float *native, msp_pid_t *piditem, float scale, unsigned numelem)
|
||||
{
|
||||
for(unsigned i = 0; i < numelem; ++i) {
|
||||
for (unsigned i = 0; i < numelem; ++i) {
|
||||
piditem->values[i] = lroundf(native[i] * scale);
|
||||
}
|
||||
}
|
||||
|
||||
static void pid_msp2native(const msp_pid_t *piditem, float *native, float scale, unsigned numelem)
|
||||
{
|
||||
for(unsigned i = 0; i < numelem; ++i) {
|
||||
for (unsigned i = 0; i < numelem; ++i) {
|
||||
native[i] = (float)piditem->values[i] / scale;
|
||||
}
|
||||
}
|
||||
@ -699,23 +699,24 @@ static UAVObjHandle get_current_pid_bank_handle()
|
||||
StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM];
|
||||
StabilizationSettingsFlightModeMapGet(flightModeMap);
|
||||
|
||||
switch(flightModeMap[fm])
|
||||
{
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1:
|
||||
return StabilizationSettingsBank1Handle();
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2:
|
||||
return StabilizationSettingsBank2Handle();
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3:
|
||||
return StabilizationSettingsBank3Handle();
|
||||
switch (flightModeMap[fm]) {
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1:
|
||||
return StabilizationSettingsBank1Handle();
|
||||
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2:
|
||||
return StabilizationSettingsBank2Handle();
|
||||
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3:
|
||||
return StabilizationSettingsBank3Handle();
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void msp_send_pid(struct msp_bridge *m)
|
||||
{
|
||||
m->current_pid_bank = get_current_pid_bank_handle();
|
||||
|
||||
|
||||
StabilizationBankData bankData;
|
||||
UAVObjGetData(m->current_pid_bank, &bankData);
|
||||
|
||||
@ -736,7 +737,7 @@ static void msp_send_pid(struct msp_bridge *m)
|
||||
|
||||
static void msp_set_pid(struct msp_bridge *m)
|
||||
{
|
||||
if(m->current_pid_bank == 0) {
|
||||
if (m->current_pid_bank == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -752,18 +753,18 @@ static void msp_set_pid(struct msp_bridge *m)
|
||||
pid_msp2native(&m->cmd_data.piditems[PIDAYAW], (float *)&bankData.YawPI, 10, 2);
|
||||
|
||||
UAVObjSetData(m->current_pid_bank, &bankData);
|
||||
|
||||
|
||||
bool needSave = true;
|
||||
|
||||
if(needSave) {
|
||||
|
||||
if (needSave) {
|
||||
FlightStatusArmedOptions armed;
|
||||
FlightStatusArmedGet(&armed);
|
||||
|
||||
if(armed == FLIGHTSTATUS_ARMED_DISARMED) {
|
||||
|
||||
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
|
||||
UAVObjSave(m->current_pid_bank, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
msp_send(m, MSP_SET_PID, 0, 0); // send ack.
|
||||
}
|
||||
|
||||
@ -806,10 +807,10 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m)
|
||||
sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR
|
||||
|
||||
// NOTE: LP alarm severity levels and MSP levels do not match. ERROR and CRITICAL are swapped.
|
||||
// So far, MW-OSD code (MSP consumer) does not make difference between ALARM_ERROR and ALARM_CRITICAL.
|
||||
// ALARM_WARN should be blinking if thats the highest severity level at the moment.
|
||||
// There might be other types of MSP consumers.
|
||||
|
||||
// So far, MW-OSD code (MSP consumer) does not make difference between ALARM_ERROR and ALARM_CRITICAL.
|
||||
// ALARM_WARN should be blinking if thats the highest severity level at the moment.
|
||||
// There might be other types of MSP consumers.
|
||||
|
||||
switch (state) {
|
||||
case SYSTEMALARMS_ALARM_WARNING:
|
||||
data.alarm.state = ALARM_WARN;
|
||||
|
@ -45,6 +45,9 @@
|
||||
// Running moving average smoothing factor
|
||||
#define PIOS_MS5611_TEMP_SMOOTHING 10
|
||||
//
|
||||
|
||||
#define PIOS_MS5611_I2C_RETRIES 5
|
||||
|
||||
/* Local Types */
|
||||
typedef struct {
|
||||
uint16_t C[6];
|
||||
@ -94,6 +97,8 @@ static const struct pios_ms5611_cfg *dev_cfg;
|
||||
static int32_t i2c_id;
|
||||
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
|
||||
|
||||
static bool sensorIsAlive = false;
|
||||
|
||||
// sensor driver interface
|
||||
bool PIOS_MS5611_driver_Test(uintptr_t context);
|
||||
void PIOS_MS5611_driver_Reset(uintptr_t context);
|
||||
@ -116,13 +121,21 @@ const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
|
||||
int32_t ms5611_read_flag;
|
||||
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
|
||||
{
|
||||
i2c_id = i2c_device;
|
||||
static uint32_t initTime;
|
||||
|
||||
oversampling = cfg->oversampling;
|
||||
conversionDelayMs = PIOS_MS5611_GetDelay();
|
||||
conversionDelayUs = PIOS_MS5611_GetDelayUs();
|
||||
if (cfg) {
|
||||
i2c_id = i2c_device;
|
||||
|
||||
dev_cfg = cfg; // Store cfg before enabling interrupt
|
||||
oversampling = cfg->oversampling;
|
||||
conversionDelayMs = PIOS_MS5611_GetDelay();
|
||||
conversionDelayUs = PIOS_MS5611_GetDelayUs();
|
||||
|
||||
dev_cfg = cfg; // Store cfg before enabling interrupt
|
||||
} else if (PIOS_DELAY_DiffuS(initTime) < 1000000) { // Do not reinitialize too often
|
||||
return;
|
||||
}
|
||||
|
||||
initTime = PIOS_DELAY_GetRaw();
|
||||
|
||||
PIOS_MS5611_WriteCommand(MS5611_RESET);
|
||||
PIOS_DELAY_WaitmS(20);
|
||||
@ -133,27 +146,35 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
|
||||
compensation_t2 = 0;
|
||||
/* Calibration parameters */
|
||||
for (int i = 0; i < 6; i++) {
|
||||
while (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2)) {}
|
||||
;
|
||||
if (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2) != 0) {
|
||||
return;
|
||||
}
|
||||
CalibData.C[i] = (data[0] << 8) | data[1];
|
||||
}
|
||||
|
||||
sensorIsAlive = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the ADC conversion
|
||||
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
|
||||
* \return 0 for success, -1 for failure (conversion completed and not read)
|
||||
* \return 0 for success, -1 for failure (conversion completed and not read), -2 if failure occurred
|
||||
*/
|
||||
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
|
||||
{
|
||||
if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */
|
||||
return -2;
|
||||
}
|
||||
|
||||
/* Start the conversion */
|
||||
|
||||
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
|
||||
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
|
||||
continue;
|
||||
if (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
|
||||
return -2;
|
||||
}
|
||||
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
|
||||
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
|
||||
continue;
|
||||
if (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
|
||||
return -2;
|
||||
}
|
||||
}
|
||||
lastConversionStart = PIOS_DELAY_GetRaw();
|
||||
@ -222,6 +243,10 @@ static uint32_t PIOS_MS5611_GetDelayUs()
|
||||
*/
|
||||
int32_t PIOS_MS5611_ReadADC(void)
|
||||
{
|
||||
if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */
|
||||
return -2;
|
||||
}
|
||||
|
||||
uint8_t Data[3];
|
||||
|
||||
Data[0] = 0;
|
||||
@ -350,7 +375,15 @@ static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
|
||||
}
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
|
||||
for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) {
|
||||
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
sensorIsAlive = false;
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -373,7 +406,15 @@ static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
|
||||
,
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
|
||||
for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) {
|
||||
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
sensorIsAlive = false;
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -435,6 +476,10 @@ bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
|
||||
static uint8_t temp_press_interleave_count = 1;
|
||||
static MS5611_FSM_State next_state = MS5611_FSM_INIT;
|
||||
|
||||
if (!sensorIsAlive) { // try to reinit
|
||||
PIOS_MS5611_Init(0, 0);
|
||||
}
|
||||
|
||||
int32_t conversionResult = PIOS_MS5611_ReadADC();
|
||||
|
||||
if (__builtin_expect(conversionResult == -1, 1)) {
|
||||
|
@ -77,12 +77,12 @@ typedef enum {
|
||||
Unvalid_RIFF_ID,
|
||||
Unvalid_WAVE_Format,
|
||||
Unvalid_FormatChunk_ID,
|
||||
Unsupporetd_FormatTag,
|
||||
Unsupporetd_Number_Of_Channel,
|
||||
Unsupporetd_Sample_Rate,
|
||||
Unsupporetd_Bits_Per_Sample,
|
||||
Unsupported_FormatTag,
|
||||
Unsupported_Number_Of_Channel,
|
||||
Unsupported_Sample_Rate,
|
||||
Unsupported_Bits_Per_Sample,
|
||||
Unvalid_DataChunk_ID,
|
||||
Unsupporetd_ExtraFormatBytes,
|
||||
Unsupported_ExtraFormatBytes,
|
||||
Unvalid_FactChunk_ID
|
||||
} ErrorCode;
|
||||
|
||||
@ -256,13 +256,13 @@ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uin
|
||||
/* Read the audio format, must be 0x01 (PCM) -------------------------------*/
|
||||
WAVE_Format.FormatTag = ReadUnit(buffer1, 20, 2, LittleEndian);
|
||||
if (WAVE_Format.FormatTag != WAVE_FORMAT_PCM) {
|
||||
return Unsupporetd_FormatTag;
|
||||
return Unsupported_FormatTag;
|
||||
}
|
||||
|
||||
/* Read the number of channels, must be 0x01 (Mono) ------------------------*/
|
||||
WAVE_Format.NumChannels = ReadUnit(buffer1, 22, 2, LittleEndian);
|
||||
if (WAVE_Format.NumChannels != CHANNEL_MONO) {
|
||||
return Unsupporetd_Number_Of_Channel;
|
||||
return Unsupported_Number_Of_Channel;
|
||||
}
|
||||
|
||||
/* Read the Sample Rate ----------------------------------------------------*/
|
||||
@ -282,7 +282,7 @@ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uin
|
||||
TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 44100;
|
||||
break; /* 44.1KHz = 24MHz / 544 */
|
||||
default:
|
||||
return Unsupporetd_Sample_Rate;
|
||||
return Unsupported_Sample_Rate;
|
||||
}
|
||||
|
||||
/* Read the Byte Rate ------------------------------------------------------*/
|
||||
@ -294,7 +294,7 @@ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uin
|
||||
/* Read the number of bits per sample --------------------------------------*/
|
||||
WAVE_Format.BitsPerSample = ReadUnit(buffer1, 34, 2, LittleEndian);
|
||||
if (WAVE_Format.BitsPerSample != BITS_PER_SAMPLE_8) {
|
||||
return Unsupporetd_Bits_Per_Sample;
|
||||
return Unsupported_Bits_Per_Sample;
|
||||
}
|
||||
SpeechDataOffset = 36;
|
||||
/* If there is Extra format bytes, these bytes will be defined in "Fact Chunk" */
|
||||
@ -302,7 +302,7 @@ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uin
|
||||
/* Read th Extra format bytes, must be 0x00 ------------------------------*/
|
||||
Temp = ReadUnit(buffer1, 36, 2, LittleEndian);
|
||||
if (Temp != 0x00) {
|
||||
return Unsupporetd_ExtraFormatBytes;
|
||||
return Unsupported_ExtraFormatBytes;
|
||||
}
|
||||
/* Read the Fact chunk, must be 'fact' -----------------------------------*/
|
||||
Temp = ReadUnit(buffer1, 38, 4, BigEndian);
|
||||
|
@ -1,5 +1,5 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
****************************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_USB_DEFS USB standard types and definitions
|
||||
@ -7,12 +7,12 @@
|
||||
* @{
|
||||
*
|
||||
* @file pios_usb_defs.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief USB Standard types and 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
|
||||
@ -358,8 +358,9 @@ enum usb_product_ids {
|
||||
USB_PRODUCT_ID_OPLINK = 0x415C,
|
||||
USB_PRODUCT_ID_CC3D = 0x415D,
|
||||
USB_PRODUCT_ID_REVOLUTION = 0x415E,
|
||||
USB_PRODUCT_ID_OSD = 0x4194,
|
||||
USB_PRODUCT_ID_SPARE = 0x4195,
|
||||
USB_PRODUCT_ID_SPARKY2 = 0x41D0, // was 0x415E during LP testing
|
||||
USB_PRODUCT_ID_OSD = 0x4194,
|
||||
USB_PRODUCT_ID_SPARE = 0x4195,
|
||||
} __attribute__((packed));
|
||||
|
||||
enum usb_op_board_ids {
|
||||
@ -368,7 +369,8 @@ enum usb_op_board_ids {
|
||||
USB_OP_BOARD_ID_OPLINK = 3,
|
||||
USB_OP_BOARD_ID_COPTERCONTROL = 4,
|
||||
USB_OP_BOARD_ID_REVOLUTION = 5,
|
||||
USB_OP_BOARD_ID_OSD = 6,
|
||||
USB_OP_BOARD_ID_SPARKY2 = 5,
|
||||
USB_OP_BOARD_ID_OSD = 6,
|
||||
} __attribute__((packed));
|
||||
|
||||
enum usb_op_board_modes {
|
||||
|
@ -0,0 +1,560 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file system_stm32f4xx.c
|
||||
* @author MCD Application Team
|
||||
* @version V1.1.0
|
||||
* @date 11-January-2013
|
||||
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
|
||||
* This file contains the system clock configuration for STM32F4xx devices,
|
||||
* and is generated by the clock configuration tool
|
||||
* stm32f4xx_Clock_Configuration_V1.1.0.xls
|
||||
*
|
||||
* 1. This file provides two functions and one global variable to be called from
|
||||
* user application:
|
||||
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
|
||||
* and Divider factors, AHB/APBx prescalers and Flash settings),
|
||||
* depending on the configuration made in the clock xls tool.
|
||||
* This function is called at startup just after reset and
|
||||
* before branch to main program. This call is made inside
|
||||
* the "startup_stm32f4xx.s" file.
|
||||
*
|
||||
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
|
||||
* by the user application to setup the SysTick
|
||||
* timer or configure other parameters.
|
||||
*
|
||||
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
|
||||
* be called whenever the core clock is changed
|
||||
* during program execution.
|
||||
*
|
||||
* 2. After each device reset the HSI (16 MHz) is used as system clock source.
|
||||
* Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to
|
||||
* configure the system clock before to branch to main program.
|
||||
*
|
||||
* 3. If the system clock source selected by user fails to startup, the SystemInit()
|
||||
* function will do nothing and HSI still used as system clock source. User can
|
||||
* add some code to deal with this issue inside the SetSysClock() function.
|
||||
*
|
||||
* 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define
|
||||
* in "stm32f4xx.h" file. When HSE is used as system clock source, directly or
|
||||
* through PLL, and you are using different crystal you have to adapt the HSE
|
||||
* value to your own configuration.
|
||||
*
|
||||
* 5. This file configures the system clock as follows:
|
||||
*=============================================================================
|
||||
*=============================================================================
|
||||
* Supported STM32F40xx/41xx/427x/437x devices
|
||||
*-----------------------------------------------------------------------------
|
||||
* System Clock source | PLL (HSE)
|
||||
*-----------------------------------------------------------------------------
|
||||
* SYSCLK(Hz) | 168000000
|
||||
*-----------------------------------------------------------------------------
|
||||
* HCLK(Hz) | 168000000
|
||||
*-----------------------------------------------------------------------------
|
||||
* AHB Prescaler | 1
|
||||
*-----------------------------------------------------------------------------
|
||||
* APB1 Prescaler | 4
|
||||
*-----------------------------------------------------------------------------
|
||||
* APB2 Prescaler | 2
|
||||
*-----------------------------------------------------------------------------
|
||||
* HSE Frequency(Hz) | 8000000
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLL_M | 10
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLL_N | 420
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLL_P | 2
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLL_Q | 7
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLLI2S_N | NA
|
||||
*-----------------------------------------------------------------------------
|
||||
* PLLI2S_R | NA
|
||||
*-----------------------------------------------------------------------------
|
||||
* I2S input clock | NA
|
||||
*-----------------------------------------------------------------------------
|
||||
* VDD(V) | 3.3
|
||||
*-----------------------------------------------------------------------------
|
||||
* Main regulator output voltage | Scale1 mode
|
||||
*-----------------------------------------------------------------------------
|
||||
* Flash Latency(WS) | 5
|
||||
*-----------------------------------------------------------------------------
|
||||
* Prefetch Buffer | ON
|
||||
*-----------------------------------------------------------------------------
|
||||
* Instruction cache | ON
|
||||
*-----------------------------------------------------------------------------
|
||||
* Data cache | ON
|
||||
*-----------------------------------------------------------------------------
|
||||
* Require 48MHz for USB OTG FS, | Enabled
|
||||
* SDIO and RNG clock |
|
||||
*-----------------------------------------------------------------------------
|
||||
*=============================================================================
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© COPYRIGHT 2013 STMicroelectronics</center></h2>
|
||||
*
|
||||
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
|
||||
* You may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at:
|
||||
*
|
||||
* http://www.st.com/software_license_agreement_liberty_v2
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/** @addtogroup CMSIS
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @addtogroup stm32f4xx_system
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_Includes
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include "stm32f4xx.h"
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/************************* Miscellaneous Configuration ************************/
|
||||
/*!< Uncomment the following line if you need to use external SRAM mounted
|
||||
on STM324xG_EVAL/STM324x7I_EVAL boards as data memory */
|
||||
/* #define DATA_IN_ExtSRAM */
|
||||
|
||||
/*!< Uncomment the following line if you need to relocate your vector Table in
|
||||
Internal SRAM. */
|
||||
/* #define VECT_TAB_SRAM */
|
||||
#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.
|
||||
This value must be a multiple of 0x200. */
|
||||
/******************************************************************************/
|
||||
|
||||
/************************* PLL Parameters *************************************/
|
||||
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
|
||||
#define PLL_M 10
|
||||
#define PLL_N 420
|
||||
|
||||
/* SYSCLK = PLL_VCO / PLL_P */
|
||||
#define PLL_P 2
|
||||
|
||||
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
|
||||
#define PLL_Q 7
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_Macros
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_Variables
|
||||
* @{
|
||||
*/
|
||||
|
||||
uint32_t SystemCoreClock = 168000000;
|
||||
|
||||
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
|
||||
* @{
|
||||
*/
|
||||
|
||||
static void SetSysClock(void);
|
||||
#ifdef DATA_IN_ExtSRAM
|
||||
static void SystemInit_ExtMemCtl(void);
|
||||
#endif /* DATA_IN_ExtSRAM */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F4xx_System_Private_Functions
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Setup the microcontroller system
|
||||
* Initialize the Embedded Flash Interface, the PLL and update the
|
||||
* SystemFrequency variable.
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void SystemInit(void)
|
||||
{
|
||||
/* FPU settings ------------------------------------------------------------*/
|
||||
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
|
||||
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
|
||||
#endif
|
||||
/* Reset the RCC clock configuration to the default reset state ------------*/
|
||||
/* Set HSION bit */
|
||||
RCC->CR |= (uint32_t)0x00000001;
|
||||
|
||||
/* Reset CFGR register */
|
||||
RCC->CFGR = 0x00000000;
|
||||
|
||||
/* Reset HSEON, CSSON and PLLON bits */
|
||||
RCC->CR &= (uint32_t)0xFEF6FFFF;
|
||||
|
||||
/* Reset PLLCFGR register */
|
||||
RCC->PLLCFGR = 0x24003010;
|
||||
|
||||
/* Reset HSEBYP bit */
|
||||
RCC->CR &= (uint32_t)0xFFFBFFFF;
|
||||
|
||||
/* Disable all interrupts */
|
||||
RCC->CIR = 0x00000000;
|
||||
|
||||
#ifdef DATA_IN_ExtSRAM
|
||||
SystemInit_ExtMemCtl();
|
||||
#endif /* DATA_IN_ExtSRAM */
|
||||
|
||||
/* Configure the System clock source, PLL Multiplier and Divider factors,
|
||||
AHB/APBx prescalers and Flash settings ----------------------------------*/
|
||||
SetSysClock();
|
||||
|
||||
/* Configure the Vector Table location add offset address ------------------*/
|
||||
#ifdef VECT_TAB_SRAM
|
||||
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
|
||||
#else
|
||||
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update SystemCoreClock variable according to Clock Register Values.
|
||||
* The SystemCoreClock variable contains the core clock (HCLK), it can
|
||||
* be used by the user application to setup the SysTick timer or configure
|
||||
* other parameters.
|
||||
*
|
||||
* @note Each time the core clock (HCLK) changes, this function must be called
|
||||
* to update SystemCoreClock variable value. Otherwise, any configuration
|
||||
* based on this variable will be incorrect.
|
||||
*
|
||||
* @note - The system frequency computed by this function is not the real
|
||||
* frequency in the chip. It is calculated based on the predefined
|
||||
* constant and the selected clock source:
|
||||
*
|
||||
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
|
||||
*
|
||||
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
|
||||
*
|
||||
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
|
||||
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
|
||||
*
|
||||
* (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value
|
||||
* 16 MHz) but the real value may vary depending on the variations
|
||||
* in voltage and temperature.
|
||||
*
|
||||
* (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value
|
||||
* 25 MHz), user has to ensure that HSE_VALUE is same as the real
|
||||
* frequency of the crystal used. Otherwise, this function may
|
||||
* have wrong result.
|
||||
*
|
||||
* - The result of this function could be not correct when using fractional
|
||||
* value for HSE crystal.
|
||||
*
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void SystemCoreClockUpdate(void)
|
||||
{
|
||||
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
|
||||
|
||||
/* Get SYSCLK source -------------------------------------------------------*/
|
||||
tmp = RCC->CFGR & RCC_CFGR_SWS;
|
||||
|
||||
switch (tmp)
|
||||
{
|
||||
case 0x00: /* HSI used as system clock source */
|
||||
SystemCoreClock = HSI_VALUE;
|
||||
break;
|
||||
case 0x04: /* HSE used as system clock source */
|
||||
SystemCoreClock = HSE_VALUE;
|
||||
break;
|
||||
case 0x08: /* PLL used as system clock source */
|
||||
|
||||
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
|
||||
SYSCLK = PLL_VCO / PLL_P
|
||||
*/
|
||||
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
|
||||
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
|
||||
|
||||
if (pllsource != 0)
|
||||
{
|
||||
/* HSE used as PLL clock source */
|
||||
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* HSI used as PLL clock source */
|
||||
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
|
||||
}
|
||||
|
||||
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
|
||||
SystemCoreClock = pllvco/pllp;
|
||||
break;
|
||||
default:
|
||||
SystemCoreClock = HSI_VALUE;
|
||||
break;
|
||||
}
|
||||
/* Compute HCLK frequency --------------------------------------------------*/
|
||||
/* Get HCLK prescaler */
|
||||
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
|
||||
/* HCLK frequency */
|
||||
SystemCoreClock >>= tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
|
||||
* AHB/APBx prescalers and Flash settings
|
||||
* @Note This function should be called only once the RCC clock configuration
|
||||
* is reset to the default reset state (done in SystemInit() function).
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void SetSysClock(void)
|
||||
{
|
||||
/******************************************************************************/
|
||||
/* PLL (clocked by HSE) used as System clock source */
|
||||
/******************************************************************************/
|
||||
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
|
||||
|
||||
/* Enable HSE */
|
||||
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
|
||||
|
||||
/* Wait till HSE is ready and if Time out is reached exit */
|
||||
do
|
||||
{
|
||||
HSEStatus = RCC->CR & RCC_CR_HSERDY;
|
||||
StartUpCounter++;
|
||||
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
|
||||
|
||||
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
|
||||
{
|
||||
HSEStatus = (uint32_t)0x01;
|
||||
}
|
||||
else
|
||||
{
|
||||
HSEStatus = (uint32_t)0x00;
|
||||
}
|
||||
|
||||
if (HSEStatus == (uint32_t)0x01)
|
||||
{
|
||||
/* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
|
||||
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
|
||||
PWR->CR |= PWR_CR_VOS;
|
||||
|
||||
/* HCLK = SYSCLK / 1*/
|
||||
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
|
||||
|
||||
/* PCLK2 = HCLK / 2*/
|
||||
RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
|
||||
|
||||
/* PCLK1 = HCLK / 4*/
|
||||
RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
|
||||
|
||||
/* Configure the main PLL */
|
||||
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
|
||||
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
|
||||
|
||||
/* Enable the main PLL */
|
||||
RCC->CR |= RCC_CR_PLLON;
|
||||
|
||||
/* Wait till the main PLL is ready */
|
||||
while((RCC->CR & RCC_CR_PLLRDY) == 0)
|
||||
{
|
||||
}
|
||||
|
||||
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
|
||||
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS;
|
||||
|
||||
/* Select the main PLL as system clock source */
|
||||
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
|
||||
RCC->CFGR |= RCC_CFGR_SW_PLL;
|
||||
|
||||
/* Wait till the main PLL is used as system clock source */
|
||||
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
|
||||
{
|
||||
}
|
||||
}
|
||||
else
|
||||
{ /* If HSE fails to start-up, the application will have wrong clock
|
||||
configuration. User can add here some code to deal with this error */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Setup the external memory controller. Called in startup_stm32f4xx.s
|
||||
* before jump to __main
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
#ifdef DATA_IN_ExtSRAM
|
||||
/**
|
||||
* @brief Setup the external memory controller.
|
||||
* Called in startup_stm32f4xx.s before jump to main.
|
||||
* This function configures the external SRAM mounted on STM324xG_EVAL/STM324x7I boards
|
||||
* This SRAM will be used as program data memory (including heap and stack).
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void SystemInit_ExtMemCtl(void)
|
||||
{
|
||||
/*-- GPIOs Configuration -----------------------------------------------------*/
|
||||
/*
|
||||
+-------------------+--------------------+------------------+------------------+
|
||||
+ SRAM pins assignment +
|
||||
+-------------------+--------------------+------------------+------------------+
|
||||
| PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 |
|
||||
| PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 |
|
||||
| PD4 <-> FSMC_NOE | PE2 <-> FSMC_A23 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 |
|
||||
| PD5 <-> FSMC_NWE | PE3 <-> FSMC_A19 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 |
|
||||
| PD8 <-> FSMC_D13 | PE4 <-> FSMC_A20 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 |
|
||||
| PD9 <-> FSMC_D14 | PE5 <-> FSMC_A21 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 |
|
||||
| PD10 <-> FSMC_D15 | PE6 <-> FSMC_A22 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 |
|
||||
| PD11 <-> FSMC_A16 | PE7 <-> FSMC_D4 | PF13 <-> FSMC_A7 |------------------+
|
||||
| PD12 <-> FSMC_A17 | PE8 <-> FSMC_D5 | PF14 <-> FSMC_A8 |
|
||||
| PD13 <-> FSMC_A18 | PE9 <-> FSMC_D6 | PF15 <-> FSMC_A9 |
|
||||
| PD14 <-> FSMC_D0 | PE10 <-> FSMC_D7 |------------------+
|
||||
| PD15 <-> FSMC_D1 | PE11 <-> FSMC_D8 |
|
||||
+-------------------| PE12 <-> FSMC_D9 |
|
||||
| PE13 <-> FSMC_D10 |
|
||||
| PE14 <-> FSMC_D11 |
|
||||
| PE15 <-> FSMC_D12 |
|
||||
+--------------------+
|
||||
*/
|
||||
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
|
||||
RCC->AHB1ENR |= 0x00000078;
|
||||
|
||||
/* Connect PDx pins to FSMC Alternate function */
|
||||
GPIOD->AFR[0] = 0x00cc00cc;
|
||||
GPIOD->AFR[1] = 0xcccccccc;
|
||||
/* Configure PDx pins in Alternate function mode */
|
||||
GPIOD->MODER = 0xaaaa0a0a;
|
||||
/* Configure PDx pins speed to 100 MHz */
|
||||
GPIOD->OSPEEDR = 0xffff0f0f;
|
||||
/* Configure PDx pins Output type to push-pull */
|
||||
GPIOD->OTYPER = 0x00000000;
|
||||
/* No pull-up, pull-down for PDx pins */
|
||||
GPIOD->PUPDR = 0x00000000;
|
||||
|
||||
/* Connect PEx pins to FSMC Alternate function */
|
||||
GPIOE->AFR[0] = 0xcccccccc;
|
||||
GPIOE->AFR[1] = 0xcccccccc;
|
||||
/* Configure PEx pins in Alternate function mode */
|
||||
GPIOE->MODER = 0xaaaaaaaa;
|
||||
/* Configure PEx pins speed to 100 MHz */
|
||||
GPIOE->OSPEEDR = 0xffffffff;
|
||||
/* Configure PEx pins Output type to push-pull */
|
||||
GPIOE->OTYPER = 0x00000000;
|
||||
/* No pull-up, pull-down for PEx pins */
|
||||
GPIOE->PUPDR = 0x00000000;
|
||||
|
||||
/* Connect PFx pins to FSMC Alternate function */
|
||||
GPIOF->AFR[0] = 0x00cccccc;
|
||||
GPIOF->AFR[1] = 0xcccc0000;
|
||||
/* Configure PFx pins in Alternate function mode */
|
||||
GPIOF->MODER = 0xaa000aaa;
|
||||
/* Configure PFx pins speed to 100 MHz */
|
||||
GPIOF->OSPEEDR = 0xff000fff;
|
||||
/* Configure PFx pins Output type to push-pull */
|
||||
GPIOF->OTYPER = 0x00000000;
|
||||
/* No pull-up, pull-down for PFx pins */
|
||||
GPIOF->PUPDR = 0x00000000;
|
||||
|
||||
/* Connect PGx pins to FSMC Alternate function */
|
||||
GPIOG->AFR[0] = 0x00cccccc;
|
||||
GPIOG->AFR[1] = 0x000000c0;
|
||||
/* Configure PGx pins in Alternate function mode */
|
||||
GPIOG->MODER = 0x00080aaa;
|
||||
/* Configure PGx pins speed to 100 MHz */
|
||||
GPIOG->OSPEEDR = 0x000c0fff;
|
||||
/* Configure PGx pins Output type to push-pull */
|
||||
GPIOG->OTYPER = 0x00000000;
|
||||
/* No pull-up, pull-down for PGx pins */
|
||||
GPIOG->PUPDR = 0x00000000;
|
||||
|
||||
/*-- FSMC Configuration ------------------------------------------------------*/
|
||||
/* Enable the FSMC interface clock */
|
||||
RCC->AHB3ENR |= 0x00000001;
|
||||
|
||||
/* Configure and enable Bank1_SRAM2 */
|
||||
FSMC_Bank1->BTCR[2] = 0x00001011;
|
||||
FSMC_Bank1->BTCR[3] = 0x00000201;
|
||||
FSMC_Bank1E->BWTR[2] = 0x0fffffff;
|
||||
/*
|
||||
Bank1_SRAM2 is configured as follow:
|
||||
|
||||
p.FSMC_AddressSetupTime = 1;
|
||||
p.FSMC_AddressHoldTime = 0;
|
||||
p.FSMC_DataSetupTime = 2;
|
||||
p.FSMC_BusTurnAroundDuration = 0;
|
||||
p.FSMC_CLKDivision = 0;
|
||||
p.FSMC_DataLatency = 0;
|
||||
p.FSMC_AccessMode = FSMC_AccessMode_A;
|
||||
|
||||
FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
|
||||
FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM;
|
||||
FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
|
||||
FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
|
||||
FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
|
||||
FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
|
||||
*/
|
||||
}
|
||||
#endif /* DATA_IN_ExtSRAM */
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
|
@ -129,6 +129,25 @@ int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg)
|
||||
/* Enable Interrupts */
|
||||
NVIC_Init(&cfg->irq.init);
|
||||
|
||||
|
||||
// Advanced timers TIM1 & TIM8 need special handling:
|
||||
// There are up to 4 separate interrupts handlers for each advanced timer, but
|
||||
// pios_tim_clock_cfg has provision for only one irq init, so we take care here
|
||||
// to enable additional irq channels that we intend to use.
|
||||
|
||||
|
||||
if (cfg->timer == TIM1) {
|
||||
NVIC_InitTypeDef init = cfg->irq.init;
|
||||
init.NVIC_IRQChannel = TIM1_UP_TIM10_IRQn;
|
||||
NVIC_Init(&init);
|
||||
#if !defined(STM32F401xx) && !defined(STM32F411xE)
|
||||
} else if (cfg->timer == TIM8) {
|
||||
NVIC_InitTypeDef init = cfg->irq.init;
|
||||
init.NVIC_IRQChannel = TIM8_UP_TIM13_IRQn;
|
||||
NVIC_Init(&init);
|
||||
#endif
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -376,12 +395,6 @@ static void PIOS_TIM_7_irq_handler(void)
|
||||
PIOS_TIM_generic_irq_handler(TIM7);
|
||||
}
|
||||
|
||||
void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler")));
|
||||
static void PIOS_TIM_8_UP_irq_handler(void)
|
||||
{
|
||||
PIOS_TIM_generic_irq_handler(TIM8);
|
||||
}
|
||||
|
||||
void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler")));
|
||||
static void PIOS_TIM_8_CC_irq_handler(void)
|
||||
{
|
||||
|
@ -863,7 +863,7 @@ void PIOS_Board_Init(void)
|
||||
OPLinkStatusSet(&oplinkStatus);
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM)
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
|
||||
|
||||
const struct pios_servo_cfg *pios_servo_cfg;
|
||||
// default to servo outputs only
|
||||
|
@ -944,7 +944,7 @@ void PIOS_Board_Init(void)
|
||||
OPLinkStatusSet(&oplinkStatus);
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM)
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
|
||||
const struct pios_servo_cfg *pios_servo_cfg;
|
||||
// default to servo outputs only
|
||||
pios_servo_cfg = &pios_servo_cfg_out;
|
||||
|
@ -775,7 +775,7 @@ void PIOS_Board_Init(void)
|
||||
} /* hwsettings_rm_flexiport */
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM)
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
|
||||
|
||||
const struct pios_servo_cfg *pios_servo_cfg;
|
||||
// default to servo outputs only
|
||||
|
@ -33,7 +33,7 @@
|
||||
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
|
||||
#include <pios_usb_util.h> /* PIOS_USB_UTIL_AsciiToUtf8 */
|
||||
|
||||
static const uint8_t usb_product_id[22] = {
|
||||
static const uint8_t usb_product_id[10] = {
|
||||
sizeof(usb_product_id),
|
||||
USB_DESC_TYPE_STRING,
|
||||
'N', 0,
|
||||
|
47
flight/targets/boards/sparky2/board-info.mk
Normal file
47
flight/targets/boards/sparky2/board-info.mk
Normal file
@ -0,0 +1,47 @@
|
||||
BOARD_TYPE := 0x92
|
||||
BOARD_REVISION := 0x01
|
||||
BOOTLOADER_VERSION := 0x06
|
||||
HW_TYPE := 0x00
|
||||
|
||||
MCU := cortex-m4
|
||||
CHIP := STM32F405RGT
|
||||
BOARD := STM32F4xx_RM
|
||||
MODEL := HD
|
||||
MODEL_SUFFIX :=
|
||||
|
||||
OPENOCD_JTAG_CONFIG := stlink-v2.cfg
|
||||
OPENOCD_CONFIG := stm32f4xx.stlink.cfg
|
||||
|
||||
# Flash memory map for Sparky2:
|
||||
# Sector start size use
|
||||
# 0 0x0800 0000 16k BL
|
||||
# 1 0x0800 4000 16k BL
|
||||
# 2 0x0800 8000 16k EE
|
||||
# 3 0x0800 C000 16k EE
|
||||
# 4 0x0801 0000 64k Unused
|
||||
# 5 0x0802 0000 128k FW
|
||||
# 6 0x0804 0000 128k FW
|
||||
# 7 0x0806 0000 128k FW
|
||||
# 8 0x0808 0000 128k Unused
|
||||
# 9 0x080A 0000 128k Unused
|
||||
# 10 0x080C 0000 128k Unused ..
|
||||
# 11 0x080E 0000 128k Unused
|
||||
|
||||
# Note: These must match the values in link_$(BOARD)_memory.ld
|
||||
BL_BANK_BASE := 0x08000000 # Start of bootloader flash
|
||||
BL_BANK_SIZE := 0x00008000 # Should include BD_INFO region
|
||||
|
||||
# 16KB for settings storage
|
||||
|
||||
EE_BANK_BASE := 0x08008000 # EEPROM storage area
|
||||
EE_BANK_SIZE := 0x00008000 # Size of EEPROM storage area
|
||||
|
||||
# Leave the remaining 64KB sectors for other uses
|
||||
|
||||
FW_BANK_BASE := 0x08020000 # Start of firmware flash
|
||||
FW_BANK_SIZE := 0x000A0000 # Should include FW_DESC_SIZE
|
||||
|
||||
FW_DESC_SIZE := 0x00000064
|
||||
|
||||
OSCILLATOR_FREQ := 8000000
|
||||
SYSCLK_FREQ := 168000000
|
1779
flight/targets/boards/sparky2/board_hw_defs.c
Normal file
1779
flight/targets/boards/sparky2/board_hw_defs.c
Normal file
@ -0,0 +1,1779 @@
|
||||
/**
|
||||
****************************************************************************************
|
||||
* @file board_hw_defs.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* 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 Sparky2 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
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
|
||||
static const struct pios_gpio pios_leds_v2[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_ALARM] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_4,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
// the other LED in the TL code is accessed this way
|
||||
[PIOS_LED_LINK] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
#if (0 && defined(PIOS_RFM22B_DEBUG_ON_TELEM))
|
||||
// Revo hardware config
|
||||
[PIOS_LED_D1] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_D2] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_D3] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_D4] = {
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
},
|
||||
#endif /* (0 && defined(PIOS_RFM22B_DEBUG_ON_TELEM)) */
|
||||
};
|
||||
|
||||
static const struct pios_gpio_cfg pios_led_v2_cfg = {
|
||||
.gpios = pios_leds_v2,
|
||||
.num_gpios = NELEMENTS(pios_leds_v2),
|
||||
};
|
||||
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_led_v2_cfg;
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
#include <pios_spi_priv.h>
|
||||
|
||||
/*
|
||||
* SPI1 Interface
|
||||
* Used for MPU9250 gyro, accelerometer and mag
|
||||
*/
|
||||
void PIOS_SPI_gyro_irq_handler(void);
|
||||
void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
|
||||
void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_gyro_cfg = {
|
||||
.regs = SPI1,
|
||||
.remap = GPIO_AF_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,
|
||||
},
|
||||
.use_crc = false,
|
||||
.dma = {
|
||||
.irq = {
|
||||
.flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA2_Stream0_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA2_Stream0,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_3,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralToMemory,
|
||||
.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_FIFOMode = DMA_FIFOMode_Disable,
|
||||
/* .DMA_FIFOThreshold */
|
||||
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
|
||||
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA2_Stream3,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_3,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
|
||||
.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_FIFOMode = DMA_FIFOMode_Disable,
|
||||
/* .DMA_FIFOThreshold */
|
||||
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
|
||||
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
|
||||
},
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.slave_count = 1,
|
||||
.ssel = {
|
||||
{
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_4,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* SPI3 Interface
|
||||
* Used for Flash and the RFM22B
|
||||
*/
|
||||
void PIOS_SPI_telem_flash_irq_handler(void);
|
||||
void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler")));
|
||||
void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_telem_flash_cfg = {
|
||||
.regs = SPI3,
|
||||
.remap = GPIO_AF_SPI3,
|
||||
.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_Low,
|
||||
.SPI_CPHA = SPI_CPHA_1Edge,
|
||||
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
|
||||
},
|
||||
.use_crc = false,
|
||||
.dma = {
|
||||
.irq = {
|
||||
// Note this is the stream ID that triggers interrupts (in this case RX)
|
||||
.flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Stream0_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Stream0,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_0,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralToMemory,
|
||||
.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,
|
||||
// TODO: Enable FIFO
|
||||
.DMA_FIFOMode = DMA_FIFOMode_Disable,
|
||||
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
|
||||
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
|
||||
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Stream5,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_0,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR),
|
||||
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
|
||||
.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_FIFOMode = DMA_FIFOMode_Disable,
|
||||
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
|
||||
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
|
||||
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
|
||||
},
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_12,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
.slave_count = 2,
|
||||
.ssel = {
|
||||
{ // RFM22b
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
}
|
||||
},
|
||||
{ // Flash
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
}
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_spi_telem_flash_id;
|
||||
void PIOS_SPI_telem_flash_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id);
|
||||
}
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
#include <pios_rfm22b_priv.h>
|
||||
|
||||
static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = {
|
||||
.vector = PIOS_RFM22_EXT_Int,
|
||||
.line = EXTI_Line2,
|
||||
.pin = {
|
||||
.gpio = GPIOD,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_2,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI2_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.exti = {
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line2, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Falling,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = {
|
||||
.spi_cfg = &pios_spi_telem_flash_cfg,
|
||||
.exti_cfg = &pios_exti_rfm22b_cfg,
|
||||
.RFXtalCap = 0x7f,
|
||||
.slave_num = 0,
|
||||
.gpio_direction = GPIO0_TX_GPIO1_RX,
|
||||
};
|
||||
|
||||
const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_rfm22b_rm2_cfg;
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
|
||||
#if defined(PIOS_INCLUDE_FLASH)
|
||||
#include "pios_flashfs_logfs_priv.h"
|
||||
#include "pios_flash_jedec_priv.h"
|
||||
#include "pios_flash_internal_priv.h"
|
||||
|
||||
static const struct flashfs_logfs_cfg flashfs_external_user_cfg = {
|
||||
.fs_magic = 0x99abceff,
|
||||
.total_fs_size = 0x001C0000, /* 2M bytes (32 sectors = entire chip) */
|
||||
.arena_size = 0x000E0000, /* biggest possible arena size fssize/2 */
|
||||
.slot_size = 0x00000100, /* 256 bytes */
|
||||
|
||||
.start_offset = 0x00040000, /* start offset */
|
||||
.sector_size = 0x00010000, /* 64K bytes */
|
||||
.page_size = 0x00000100, /* 256 bytes */
|
||||
};
|
||||
|
||||
static const struct flashfs_logfs_cfg flashfs_external_system_cfg = {
|
||||
.fs_magic = 0x99bbcdef,
|
||||
.total_fs_size = 0x00040000, /* 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 */
|
||||
};
|
||||
|
||||
|
||||
static const struct pios_flash_internal_cfg flash_internal_cfg = {};
|
||||
|
||||
static const struct flashfs_logfs_cfg flashfs_internal_cfg = {
|
||||
.fs_magic = 0x99abcfef,
|
||||
.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */
|
||||
.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */
|
||||
.slot_size = 0x00000100, /* 256 bytes */
|
||||
|
||||
.start_offset = EE_BANK_BASE, /* start after the bootloader */
|
||||
.sector_size = 0x00004000, /* 16K bytes */
|
||||
.page_size = 0x00004000, /* 16K bytes */
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_FLASH */
|
||||
|
||||
#include <pios_usart_priv.h>
|
||||
|
||||
#ifdef PIOS_INCLUDE_COM_TELEM
|
||||
|
||||
/*
|
||||
* MAIN PORT
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_main_cfg = {
|
||||
.regs = USART1,
|
||||
.remap = GPIO_AF_USART1,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_COM_TELEM */
|
||||
|
||||
#ifdef PIOS_INCLUDE_DSM
|
||||
|
||||
#include "pios_dsm_priv.h"
|
||||
static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
|
||||
.regs = USART1,
|
||||
.remap = GPIO_AF_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_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
// Because of the inverter on the main port this will not
|
||||
// work. Notice the mode is set to IN to maintain API
|
||||
// compatibility but protect the pins
|
||||
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_IN,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_DSM */
|
||||
|
||||
#ifdef PIOS_INCLUDE_COM_FLEXI
|
||||
/*
|
||||
* FLEXI PORT
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl =
|
||||
USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM_FLEXI */
|
||||
|
||||
#ifdef PIOS_INCLUDE_DSM
|
||||
|
||||
#include "pios_dsm_priv.h"
|
||||
static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_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_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
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,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_DSM */
|
||||
|
||||
#if defined(PIOS_INCLUDE_SRXL)
|
||||
/*
|
||||
* SRXL USART
|
||||
*/
|
||||
#include <pios_srxl_priv.h>
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_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_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_srxl_rcvr_cfg = {
|
||||
.regs = USART6,
|
||||
.remap = GPIO_AF_USART6,
|
||||
.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 = USART6_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_SRXL */
|
||||
|
||||
// these were copied from Revo support
|
||||
// they might need to be further modified for Sparky2 support
|
||||
#if defined(PIOS_INCLUDE_HOTT)
|
||||
/*
|
||||
* HOTT USART
|
||||
*/
|
||||
#include <pios_hott_priv.h>
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_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_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_hott_rcvr_cfg = {
|
||||
.regs = USART6,
|
||||
.remap = GPIO_AF_USART6,
|
||||
.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 = USART6_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_HOTT */
|
||||
|
||||
#if defined(PIOS_INCLUDE_EXBUS)
|
||||
/*
|
||||
* EXBUS USART
|
||||
*/
|
||||
#include <pios_exbus_priv.h>
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 125000,
|
||||
.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_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_exbus_rcvr_cfg = {
|
||||
.regs = USART6,
|
||||
.remap = GPIO_AF_USART6,
|
||||
.init = {
|
||||
.USART_BaudRate = 125000,
|
||||
.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 = USART6_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_EXBUS */
|
||||
|
||||
/*
|
||||
* HK OSD
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
|
||||
.regs = USART1,
|
||||
.remap = GPIO_AF_USART1,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_AF_USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
* RCVR PORT
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_SBUS)
|
||||
/*
|
||||
* S.Bus USART
|
||||
*/
|
||||
#include <pios_sbus_priv.h>
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_sbus_rcvr_cfg = {
|
||||
.regs = USART6,
|
||||
.remap = GPIO_AF_USART6,
|
||||
.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 = USART6_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
static const struct pios_sbus_cfg pios_sbus_cfg = {
|
||||
/* Inverter configuration */
|
||||
.inv = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.gpio_inv_enable = Bit_SET,
|
||||
.gpio_inv_disable = Bit_RESET,
|
||||
.gpio_clk_func = RCC_AHB1PeriphClockCmd,
|
||||
.gpio_clk_periph = RCC_AHB1Periph_GPIOC,
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_SBUS */
|
||||
|
||||
#ifdef PIOS_INCLUDE_DSM
|
||||
|
||||
// It looks like TL notes originally came from OP's pios_dsm_main_cfg
|
||||
// (TL note) Because of the inverter on the main port this will not
|
||||
// (TL note) work. Notice the mode is set to IN to maintain API
|
||||
// (TL note) compatibility but protect the pins
|
||||
static const struct pios_dsm_cfg pios_dsm_rcvr_cfg = {
|
||||
.bind = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_dsm_rcvr_cfg = {
|
||||
.regs = USART6,
|
||||
.remap = GPIO_AF_USART6,
|
||||
.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 = USART6_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_DSM */
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
|
||||
#include <pios_com_priv.h>
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
|
||||
#include <pios_i2c_priv.h>
|
||||
|
||||
/*
|
||||
* I2C Adapters
|
||||
*/
|
||||
void PIOS_i2c_mag_pressure_adapter_ev_irq_handler(void);
|
||||
void PIOS_i2c_mag_pressure_adapter_er_irq_handler(void);
|
||||
void I2C1_EV_IRQHandler()
|
||||
__attribute__((alias("PIOS_i2c_mag_pressure_adapter_ev_irq_handler")));
|
||||
void I2C1_ER_IRQHandler()
|
||||
__attribute__((alias("PIOS_i2c_mag_pressure_adapter_er_irq_handler")));
|
||||
|
||||
static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = {
|
||||
.regs = I2C1,
|
||||
.remapSCL = GPIO_AF_I2C1,
|
||||
.remapSDA = GPIO_AF_I2C1,
|
||||
.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_8,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.sda = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.event = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C1_EV_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.error = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C1_ER_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_i2c_mag_pressure_adapter_id;
|
||||
void PIOS_i2c_mag_pressure_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_mag_pressure_adapter_id);
|
||||
}
|
||||
|
||||
void PIOS_i2c_mag_pressure_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_mag_pressure_adapter_id);
|
||||
}
|
||||
|
||||
|
||||
void PIOS_I2C_flexiport_adapter_ev_irq_handler(void);
|
||||
void PIOS_I2C_flexiport_adapter_er_irq_handler(void);
|
||||
void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler")));
|
||||
void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler")));
|
||||
|
||||
static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = {
|
||||
.regs = I2C2,
|
||||
.remapSCL = GPIO_AF_I2C2,
|
||||
.remapSDA = GPIO_AF_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_Mode = GPIO_Mode_AF,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.sda = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Mode = GPIO_Mode_AF,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.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_flexiport_adapter_id;
|
||||
void PIOS_I2C_flexiport_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_flexiport_adapter_id);
|
||||
}
|
||||
|
||||
void PIOS_I2C_flexiport_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_flexiport_adapter_id);
|
||||
}
|
||||
|
||||
|
||||
void PIOS_i2c_mag_pressure_adapter_ev_irq_handler(void);
|
||||
void PIOS_i2c_mag_pressure_adapter_er_irq_handler(void);
|
||||
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
#if defined(PIOS_INCLUDE_RTC)
|
||||
/*
|
||||
* Realtime Clock (RTC)
|
||||
*/
|
||||
#include <pios_rtc_priv.h>
|
||||
|
||||
void PIOS_RTC_IRQ_Handler(void);
|
||||
void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler")));
|
||||
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
|
||||
.clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1
|
||||
// For some reason it's acting like crystal is 16 Mhz. This clock is then divided
|
||||
// by another 16 to give a nominal 62.5 khz clock
|
||||
.prescaler = 100, // Every 100 cycles gives 625 Hz
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = RTC_WKUP_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) */
|
||||
|
||||
#include "pios_tim_priv.h"
|
||||
|
||||
static const TIM_TimeBaseInitTypeDef tim_3_5_time_base = {
|
||||
.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_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 TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = {
|
||||
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_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_2_cfg = {
|
||||
.timer = TIM2,
|
||||
.time_base_init = &tim_3_5_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_3_5_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_5_cfg = {
|
||||
.timer = TIM5,
|
||||
.time_base_init = &tim_3_5_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM5_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_9_cfg = {
|
||||
.timer = TIM9,
|
||||
.time_base_init = &tim_9_10_11_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_10_cfg = {
|
||||
.timer = TIM10,
|
||||
.time_base_init = &tim_9_10_11_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM1_UP_TIM10_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_11_cfg = {
|
||||
.timer = TIM11,
|
||||
.time_base_init = &tim_9_10_11_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
// Set up timers that only have inputs on APB1
|
||||
// TIM2,3,4,5,6,7,12,13,14
|
||||
static const TIM_TimeBaseInitTypeDef tim_apb1_time_base = {
|
||||
.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1,
|
||||
.TIM_ClockDivision = TIM_CKD_DIV1,
|
||||
.TIM_CounterMode = TIM_CounterMode_Up,
|
||||
.TIM_Period = 0xFFFF,
|
||||
.TIM_RepetitionCounter = 0x0000,
|
||||
};
|
||||
|
||||
|
||||
// Set up timers that only have inputs on APB2
|
||||
// TIM1,8,9,10,11
|
||||
static const TIM_TimeBaseInitTypeDef tim_apb2_time_base = {
|
||||
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
|
||||
.TIM_ClockDivision = TIM_CKD_DIV1,
|
||||
.TIM_CounterMode = TIM_CounterMode_Up,
|
||||
.TIM_Period = 0xFFFF,
|
||||
.TIM_RepetitionCounter = 0x0000,
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_1_cfg = {
|
||||
.timer = TIM1,
|
||||
.time_base_init = &tim_apb2_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_4_cfg = {
|
||||
.timer = TIM4,
|
||||
.time_base_init = &tim_apb1_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_clock_cfg tim_8_cfg = {
|
||||
.timer = TIM8,
|
||||
.time_base_init = &tim_apb2_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM8_CC_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_12_cfg = {
|
||||
.timer = TIM12,
|
||||
.time_base_init = &tim_apb1_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#include <pios_servo_priv.h>
|
||||
#include <pios_servo_config.h>
|
||||
static const struct pios_tim_channel dummmy_timer =
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM9, 1, E, 5); // dummy unused timer + gpio. Hack to free tim1
|
||||
|
||||
/**
|
||||
* Pios servo configuration structures
|
||||
* Using TIM3, TIM9, TIM2, TIM5
|
||||
*/
|
||||
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0),
|
||||
|
||||
// PWM pins on CONN7
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15),
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14),
|
||||
};
|
||||
|
||||
#define PIOS_SERVOPORT_ALL_PINS_PWMOUT (NELEMENTS(pios_tim_servoport_all_pins))
|
||||
|
||||
const struct pios_servo_cfg pios_servo_cfg_out = {
|
||||
.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 = PIOS_SERVOPORT_ALL_PINS_PWMOUT,
|
||||
};
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
#include <pios_pwm_priv.h>
|
||||
static const struct pios_tim_channel pios_tim_rcvr_all_channels[] = {
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7),
|
||||
};
|
||||
|
||||
/*
|
||||
* PPM Input
|
||||
*/
|
||||
|
||||
#include <pios_ppm_priv.h>
|
||||
static 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,
|
||||
.TIM_Channel = TIM_Channel_2,
|
||||
},
|
||||
/* Use only the first channel for ppm */
|
||||
.channels = &pios_tim_rcvr_all_channels[0],
|
||||
.num_channels = 1,
|
||||
};
|
||||
|
||||
#endif // PPM
|
||||
|
||||
#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_rm2_cfg = {
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = OTG_FS_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0, // PriorityGroup=4
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.vsense = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
},
|
||||
},
|
||||
.vsense_active_low = false
|
||||
};
|
||||
|
||||
const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_usb_main_rm2_cfg;
|
||||
}
|
||||
|
||||
#include "pios_usb_board_data_priv.h"
|
||||
#include "pios_usb_desc_hid_cdc_priv.h"
|
||||
#include "pios_usb_desc_hid_only_priv.h"
|
||||
#include "pios_usbhook.h"
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM_MSG)
|
||||
|
||||
#include <pios_com_msg_priv.h>
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM_MSG */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC)
|
||||
#include <pios_usb_hid_priv.h>
|
||||
|
||||
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
|
||||
.data_if = 0,
|
||||
.data_rx_ep = 1,
|
||||
.data_tx_ep = 1,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC)
|
||||
#include <pios_usb_cdc_priv.h>
|
||||
|
||||
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,
|
||||
};
|
||||
|
||||
#include <pios_usb_hid_priv.h>
|
||||
|
||||
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 && PIOS_INCLUDE_USB_CDC */
|
||||
|
||||
#ifdef PIOS_INCLUDE_WS2811
|
||||
#include <pios_ws2811_cfg.h>
|
||||
#include <hwsettings.h>
|
||||
#define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD))
|
||||
|
||||
void DMA2_Stream1_IRQHandler(void) __attribute__((alias("PIOS_WS2811_irq_handler")));
|
||||
// list of pin configurable as ws281x outputs.
|
||||
// this will not clash with PWM in or servo output as
|
||||
// pins will be reconfigured as _OUT so the alternate function is disabled.
|
||||
const struct pios_ws2811_pin_cfg pios_ws2811_pin_cfg[] = {
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT1] = {
|
||||
.gpio = GPIOB,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_0,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT2] = {
|
||||
.gpio = GPIOB,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_1,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT3] = {
|
||||
.gpio = GPIOA,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT4] = {
|
||||
.gpio = GPIOA,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_2,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT5] = {
|
||||
.gpio = GPIOA,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_1,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
[HWSETTINGS_WS2811LED_OUT_SERVOOUT6] = {
|
||||
.gpio = GPIOA,
|
||||
.gpioInit = {
|
||||
.GPIO_Pin = GPIO_Pin_0,
|
||||
.GPIO_Speed = GPIO_Speed_25MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
const struct pios_ws2811_cfg pios_ws2811_cfg = {
|
||||
.timer = TIM1,
|
||||
.timerInit = {
|
||||
.TIM_Prescaler = PIOS_WS2811_TIM_DIVIDER - 1,
|
||||
.TIM_ClockDivision = TIM_CKD_DIV1,
|
||||
.TIM_CounterMode = TIM_CounterMode_Up,
|
||||
// period (1.25 uS per period
|
||||
.TIM_Period = PIOS_WS2811_TIM_PERIOD,
|
||||
.TIM_RepetitionCounter = 0x0000,
|
||||
},
|
||||
|
||||
.timerCh1 = 1,
|
||||
.streamCh1 = DMA2_Stream1,
|
||||
.timerCh2 = 3,
|
||||
.streamCh2 = DMA2_Stream6,
|
||||
.streamUpdate = DMA2_Stream5,
|
||||
|
||||
// DMA streamCh1, triggered by timerCh1 pwm signal.
|
||||
// if FrameBuffer indicates, reset output value early to indicate "0" bit to ws2812
|
||||
.dmaInitCh1 = PIOS_WS2811_DMA_CH1_CONFIG(DMA_Channel_6),
|
||||
.dmaItCh1 = DMA_IT_TEIF1 | DMA_IT_TCIF1,
|
||||
|
||||
// DMA streamCh2, triggered by timerCh2 pwm signal.
|
||||
// Reset output value late to indicate "1" bit to ws2812.
|
||||
.dmaInitCh2 = PIOS_WS2811_DMA_CH2_CONFIG(DMA_Channel_6),
|
||||
.dmaItCh2 = DMA_IT_TEIF6 | DMA_IT_TCIF6,
|
||||
|
||||
// DMA streamUpdate Triggered by timer update event
|
||||
// Outputs a high logic level at beginning of a cycle
|
||||
.dmaInitUpdate = PIOS_WS2811_DMA_UPDATE_CONFIG(DMA_Channel_6),
|
||||
.dmaItUpdate = DMA_IT_TEIF5 | DMA_IT_TCIF5,
|
||||
.dmaSource = TIM_DMA_CC1 | TIM_DMA_CC3 | TIM_DMA_Update,
|
||||
|
||||
// DMAInitCh1 interrupt vector, used to block timer at end of framebuffer transfer
|
||||
.irq = {
|
||||
.flags = (DMA_IT_TCIF1),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA2_Stream1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
void PIOS_WS2811_irq_handler(void)
|
||||
{
|
||||
PIOS_WS2811_DMA_irq_handler();
|
||||
}
|
||||
#endif // PIOS_INCLUDE_WS2811
|
27
flight/targets/boards/sparky2/bootloader/Makefile
Normal file
27
flight/targets/boards/sparky2/bootloader/Makefile
Normal file
@ -0,0 +1,27 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# 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 FLIGHT_MAKEFILE
|
||||
$(error Top level Makefile must be used to build this target)
|
||||
endif
|
||||
|
||||
include ../board-info.mk
|
||||
include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk
|
||||
include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk
|
||||
include $(FLIGHT_ROOT_DIR)/make/common-defs.mk
|
115
flight/targets/boards/sparky2/bootloader/inc/common.h
Normal file
115
flight/targets/boards/sparky2/bootloader/inc/common.h
Normal file
@ -0,0 +1,115 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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_ */
|
44
flight/targets/boards/sparky2/bootloader/inc/pios_config.h
Normal file
44
flight/targets/boards/sparky2/bootloader/inc/pios_config.h
Normal file
@ -0,0 +1,44 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_USB
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#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 */
|
@ -0,0 +1,54 @@
|
||||
/**
|
||||
***********************************************************************************
|
||||
* @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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* 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 <pios_usb_defs.h> /* struct usb_* */
|
||||
|
||||
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_SPARKY2
|
||||
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_SPARKY2, USB_OP_BOARD_MODE_BL)
|
||||
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
|
||||
|
||||
/*
|
||||
* The bootloader uses a simplified report structure
|
||||
* BL: <REPORT_ID><DATA>...<DATA>
|
||||
* FW: <REPORT_ID><LENGTH><DATA>...<DATA>
|
||||
* 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 */
|
257
flight/targets/boards/sparky2/bootloader/main.c
Normal file
257
flight/targets/boards/sparky2/bootloader/main.c
Normal file
@ -0,0 +1,257 @@
|
||||
/**
|
||||
***********************************************************************************
|
||||
* @addtogroup Sparky2BL Sparky2 BootLoader
|
||||
* @brief These files contain the code to the Sparky2 Bootloader.
|
||||
*
|
||||
* @{
|
||||
* @file main.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief This is the file with the main function of the Sparky2 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 <pios.h>
|
||||
#include <pios_board_info.h>
|
||||
#include <op_dfu.h>
|
||||
#include <pios_iap.h>
|
||||
#include <fifo_buffer.h>
|
||||
#include <pios_com_msg.h>
|
||||
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
|
||||
#include <stdbool.h>
|
||||
#include <pios_board_init.h>
|
||||
|
||||
extern void FLASH_Download();
|
||||
void check_bor();
|
||||
#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;
|
||||
bool JumpToApp = false;
|
||||
bool GO_dfu = false;
|
||||
bool USB_connected = false;
|
||||
bool 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();
|
||||
|
||||
// Make sure the brown out reset value for this chip
|
||||
// is 2.7 volts
|
||||
check_bor();
|
||||
|
||||
USB_connected = PIOS_USB_CheckAvailable(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) {
|
||||
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;
|
||||
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
// Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack
|
||||
uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000;
|
||||
// Check for the two possible irqstack locations (sram or core coupled sram)
|
||||
if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) {
|
||||
/* Jump to user application */
|
||||
FLASH_Lock();
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
|
||||
|
||||
PIOS_USBHOOK_Deactivate();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check the brown out reset threshold is 2.7 volts and if not
|
||||
* resets it. This solves an issue that can prevent boards
|
||||
* powering up with some BEC
|
||||
*/
|
||||
void check_bor()
|
||||
{
|
||||
uint8_t bor = FLASH_OB_GetBOR();
|
||||
|
||||
if (bor != OB_BOR_LEVEL3) {
|
||||
FLASH_OB_Unlock();
|
||||
FLASH_OB_BORConfig(OB_BOR_LEVEL3);
|
||||
FLASH_OB_Launch();
|
||||
while (FLASH_WaitForLastOperation() == FLASH_BUSY) {
|
||||
;
|
||||
}
|
||||
FLASH_OB_Lock();
|
||||
while (FLASH_WaitForLastOperation() == FLASH_BUSY) {
|
||||
;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len)
|
||||
{
|
||||
return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len);
|
||||
}
|
84
flight/targets/boards/sparky2/bootloader/pios_board.c
Normal file
84
flight/targets/boards/sparky2/bootloader/pios_board.c
Normal file
@ -0,0 +1,84 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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 AHRS 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 <pios.h>
|
||||
#include <pios_board_info.h>
|
||||
|
||||
/*
|
||||
* 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;
|
||||
|
||||
static bool board_init_complete = false;
|
||||
void PIOS_Board_Init()
|
||||
{
|
||||
if (board_init_complete) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
|
||||
|
||||
#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 */
|
||||
|
||||
PIOS_USBHOOK_Activate();
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
board_init_complete = true;
|
||||
}
|
115
flight/targets/boards/sparky2/firmware/Makefile
Normal file
115
flight/targets/boards/sparky2/firmware/Makefile
Normal file
@ -0,0 +1,115 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# 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 FLIGHT_MAKEFILE
|
||||
$(error Top level Makefile must be used to build this target)
|
||||
endif
|
||||
|
||||
include ../board-info.mk
|
||||
include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk
|
||||
|
||||
# Sparky2 C++ support
|
||||
USE_CXX = YES
|
||||
|
||||
# ARM DSP library
|
||||
USE_DSP_LIB ?= NO
|
||||
|
||||
# List of mandatory modules to include
|
||||
MODULES += Sensors
|
||||
MODULES += StateEstimation
|
||||
MODULES += Airspeed
|
||||
MODULES += Stabilization
|
||||
MODULES += ManualControl
|
||||
MODULES += Receiver
|
||||
MODULES += Actuator
|
||||
MODULES += GPS
|
||||
MODULES += TxPID
|
||||
MODULES += CameraStab
|
||||
MODULES += Battery
|
||||
MODULES += FirmwareIAP
|
||||
MODULES += Radio
|
||||
MODULES += PathPlanner
|
||||
MODULES += PathFollower
|
||||
MODULES += Osd/osdoutout
|
||||
MODULES += Logging
|
||||
MODULES += Telemetry
|
||||
MODULES += Notify
|
||||
|
||||
OPTMODULES += ComUsbBridge
|
||||
|
||||
SRC += $(FLIGHTLIB)/notification.c
|
||||
|
||||
# Include all camera options
|
||||
CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
|
||||
|
||||
# Some diagnostics
|
||||
CDEFS += -DDIAG_TASKS
|
||||
|
||||
# Misc options
|
||||
CFLAGS += -ffast-math
|
||||
|
||||
# List C source files here (C dependencies are automatically generated).
|
||||
# Use file-extension c for "c-only"-files
|
||||
ifndef TESTAPP
|
||||
## Application Core
|
||||
SRC += ../pios_usb_board_data.c
|
||||
SRC += $(OPMODULEDIR)/System/systemmod.c
|
||||
CPPSRC += $(OPSYSTEM)/sparky2.cpp
|
||||
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
|
||||
SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c
|
||||
SRC += $(PIOSCOMMON)/pios_flash_jedec.c
|
||||
|
||||
#ifeq ($(DEBUG), YES)
|
||||
SRC += $(OPSYSTEM)/dcc_stdio.c
|
||||
SRC += $(OPSYSTEM)/cm3_fault_handlers.c
|
||||
#endif
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/plans.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/auxmagsupport.c
|
||||
SRC += $(FLIGHTLIB)/lednotification.c
|
||||
SRC += $(FLIGHTLIB)/sha1.c
|
||||
|
||||
## UAVObjects
|
||||
include ./UAVObjects.inc
|
||||
SRC += $(UAVOBJSRC)
|
||||
else
|
||||
## Test Code
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
# Optional component libraries
|
||||
include $(FLIGHTLIB)/rscode/library.mk
|
||||
|
||||
#include $(FLIGHTLIB)/PyMite/pymite.mk
|
||||
|
||||
|
||||
include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk
|
||||
include $(FLIGHT_ROOT_DIR)/make/common-defs.mk
|
656
flight/targets/boards/sparky2/firmware/Makefile.osx
Normal file
656
flight/targets/boards/sparky2/firmware/Makefile.osx
Normal file
@ -0,0 +1,656 @@
|
||||
#####
|
||||
# Project: RevoMini
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2012.
|
||||
#
|
||||
#
|
||||
# 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
|
||||
#####
|
||||
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
#
|
||||
USE_BOOTLOADER ?= NO
|
||||
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= NO
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
REMOVE_CMD ?= rm
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# in SRC and CPPSRC
|
||||
USE_THUMB_MODE = YES
|
||||
|
||||
# List of modules to include
|
||||
MODULES += Actuator ManualControl Stabilization
|
||||
MODULES += AltitudeHold FixedWingPathFollower PathPlanner
|
||||
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
|
||||
MODULES += Attitude/revolution
|
||||
#MODULES += OveroSync/simulated
|
||||
|
||||
# To run simulation instead of connect to SITL
|
||||
MODULES += Sensors/simulated
|
||||
|
||||
MODULES += Telemetry
|
||||
|
||||
# MCU name, submodel and board
|
||||
# - MCU used for compiler-option (-mtune)
|
||||
# - MODEL used for linker-script name (-T) and passed as define
|
||||
# - BOARD just passed as define (optional)
|
||||
MCU = i686
|
||||
#CHIP = STM32F103RET
|
||||
#BOARD = STM3210E_OP
|
||||
MODEL = HD
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
BOOT_MODEL = $(MODEL)_BL
|
||||
|
||||
else
|
||||
BOOT_MODEL = $(MODEL)_NB
|
||||
endif
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR = ../../build/sim_osx
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET = RevoMini
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = ./System
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ../UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ../modules
|
||||
FLIGHTLIB = ../libraries
|
||||
FLIGHTLIBINC = $(FLIGHTLIB)/inc
|
||||
PIOS = ../PiOS.osx
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSPOSIX = $(PIOS)/osx
|
||||
APPLIBDIR = $(PIOSPOSIX)/libraries
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
PYMITE = $(FLIGHTLIB)/PyMite
|
||||
PYMITELIB = $(PYMITE)/lib
|
||||
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
|
||||
PYMITETOOLS = $(PYMITE)/tools
|
||||
PYMITEVM = $(PYMITE)/vm
|
||||
PYMITEINC = $(PYMITEVM)
|
||||
PYMITEINC += $(PYMITEPLAT)
|
||||
PYMITEINC += $(OUTDIR)
|
||||
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
|
||||
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
|
||||
|
||||
UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
|
||||
ifndef TESTAPP
|
||||
|
||||
## PyMite files
|
||||
SRC += $(OUTDIR)/pmlib_img.c
|
||||
SRC += $(OUTDIR)/pmlib_nat.c
|
||||
SRC += $(OUTDIR)/pmlibusr_img.c
|
||||
SRC += $(OUTDIR)/pmlibusr_nat.c
|
||||
SRC += $(wildcard ${PYMITEVM}/*.c)
|
||||
SRC += $(wildcard ${PYMITEPLAT}/*.c)
|
||||
|
||||
## MODULES
|
||||
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${OUTDIR}/InitMods.c
|
||||
## OPENPILOT CORE:
|
||||
SRC += ${OPMODULEDIR}/System/systemmod.c
|
||||
SRC += $(OPSYSTEM)/revolution.c
|
||||
SRC += $(OPSYSTEM)/pios_board_sim.c
|
||||
SRC += $(OPSYSTEM)/alarms.c
|
||||
SRC += $(OPUAVTALK)/uavtalk.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectmanager.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
SRC += $(FLIGHT_UAVOBJ_DIR)/uavobjectsinit.c
|
||||
else
|
||||
## TESTCODE
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
|
||||
|
||||
## UAVOBJECTS
|
||||
ifndef TESTAPP
|
||||
#include $(FLIGHT_UAVOBJ_DIR)/Makefile.inc
|
||||
include ./UAVObjects.inc
|
||||
|
||||
UAVOBJSRCFILENAMES += attitudesimulated
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
SRC += $(UAVOBJSRC)
|
||||
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
|
||||
endif
|
||||
|
||||
## PIOS Hardware (posix)
|
||||
SRC += $(PIOSPOSIX)/pios_crc.c
|
||||
SRC += $(PIOSPOSIX)/pios_sys.c
|
||||
SRC += $(PIOSPOSIX)/pios_led.c
|
||||
SRC += $(PIOSPOSIX)/pios_irq.c
|
||||
SRC += $(PIOSPOSIX)/pios_delay.c
|
||||
SRC += $(PIOSPOSIX)/pios_sdcard.c
|
||||
SRC += $(PIOSPOSIX)/pios_udp.c
|
||||
SRC += $(PIOSPOSIX)/pios_tcp.c
|
||||
SRC += $(PIOSPOSIX)/pios_com.c
|
||||
SRC += $(PIOSPOSIX)/pios_servo.c
|
||||
SRC += $(PIOSPOSIX)/pios_wdg.c
|
||||
SRC += $(PIOSPOSIX)/pios_debug.c
|
||||
|
||||
SRC += $(PIOSPOSIX)/pios_rcvr.c
|
||||
SRC += $(PIOSPOSIX)/pios_gcsrcvr.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
|
||||
## RTOS and RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
UNAME := $(shell uname)
|
||||
SRC += $(RTOSSRCDIR)/task.c
|
||||
SRC += $(RTOSSRCDIR)/timers.c
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port.c
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
|
||||
|
||||
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(FLIGHT_UAVOBJ_DIR)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSPOSIX)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Posix
|
||||
EXTRAINCDIRS += $(PYMITEINC)
|
||||
|
||||
EXTRAINCDIRS += ${foreach MOD, ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
#DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -g$(DEBUGF) -DDEBUG
|
||||
endif
|
||||
|
||||
CFLAGS += -DDIAG_TASKS
|
||||
|
||||
CFLAGS += $(CFLAGS_UAVOBJECTS)
|
||||
CFLAGS += -DARCH_POSIX
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -mtune=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
#CFLAGS += ARCH=arm
|
||||
#CROSS_COMPILE=/usr/local/android-ndk-r5/toolchains/arm-linux-androideabi-4.4.3/prebuilt/darwin-x86/bin/arm-linux-androideabi-
|
||||
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS += -lpthread
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# To include simulation model
|
||||
LDFLAGS += -L$(OUTDIR)
|
||||
#LDFLAGS += -lsimmodel
|
||||
|
||||
|
||||
# Define programs and commands.
|
||||
CC = $(ARM_SDK_PREFIX)gcc
|
||||
CPP = $(ARM_SDK_PREFIX)g++
|
||||
AR = $(ARM_SDK_PREFIX)ar
|
||||
OBJCOPY = $(ARM_SDK_PREFIX)objcopy
|
||||
OBJDUMP = $(ARM_SDK_PREFIX)objdump
|
||||
SIZE = $(ARM_SDK_PREFIX)size
|
||||
NM = $(ARM_SDK_PREFIX)nm
|
||||
REMOVE = rm -f
|
||||
|
||||
|
||||
|
||||
# Define Messages
|
||||
# English
|
||||
MSG_ERRORS_NONE = Errors: none
|
||||
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
|
||||
MSG_END = ${quote}-------- end --------${quote}
|
||||
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
|
||||
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
|
||||
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
|
||||
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
|
||||
MSG_LINKING = ${quote}**** Linking :${quote}
|
||||
MSG_COMPILING = ${quote}**** Compiling C :${quote}
|
||||
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
|
||||
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
|
||||
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
|
||||
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
|
||||
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
|
||||
MSG_CLEANING = ${quote}Cleaning project:${quote}
|
||||
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
|
||||
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
|
||||
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
|
||||
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Default target.
|
||||
#all: begin gccversion sizebefore build sizeafter finished end
|
||||
#all: begin gencode gccversion build sizeafter finished end
|
||||
all: elf
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Generate intermediate code
|
||||
gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h
|
||||
|
||||
getmodname = $(firstword $(subst /, ,$1))
|
||||
|
||||
MOD_GEN := $(foreach MOD,$(MODULES),$(call getmodname,$(MOD)))
|
||||
|
||||
# Generate code for module initialization
|
||||
${OUTDIR}/InitMods.c: Makefile.osx
|
||||
echo ${MOD_GEN}
|
||||
@echo ${MSG_MODINIT}
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MOD_GEN}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MOD_GEN}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MOD_GEN}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MOD_GEN}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
# Generate code for PyMite
|
||||
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)
|
||||
@echo ${MSG_PYMITEINIT}
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
|
||||
|
||||
# Eye candy.
|
||||
begin:
|
||||
## @echo
|
||||
@echo $(MSG_BEGIN)
|
||||
|
||||
finished:
|
||||
## @echo $(MSG_ERRORS_NONE)
|
||||
|
||||
end:
|
||||
@echo $(MSG_END)
|
||||
## @echo
|
||||
|
||||
# Display sizes of sections.
|
||||
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
|
||||
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
|
||||
sizebefore:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
|
||||
|
||||
sizeafter:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
|
||||
@echo $(MSG_SIZE_AFTER)
|
||||
$(ELFSIZE)
|
||||
|
||||
# Display compiler version information.
|
||||
gccversion :
|
||||
@$(CC) --version
|
||||
# @echo $(ALLOBJ)
|
||||
|
||||
# Program the device.
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
# Program the device with OP Upload Tool".
|
||||
program: $(OUTDIR)/$(TARGET).bin
|
||||
@echo ${quote}Programming with OP Upload Tool${quote}
|
||||
../../ground/src/experimental/upload-build-desktop/debug/OPUploadTool -d 0 -p $(OUTDIR)/$(TARGET).bin
|
||||
else
|
||||
ifeq ($(FLASH_TOOL),OPENOCD)
|
||||
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
|
||||
program: $(OUTDIR)/$(TARGET).elf
|
||||
@echo ${quote}Programming with OPENOCD${quote}
|
||||
$(OOCD_EXE) $(OOCD_CL)
|
||||
endif
|
||||
endif
|
||||
|
||||
# Create final output file (.hex) from ELF output file.
|
||||
%.hex: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O ihex $< $@
|
||||
|
||||
# Create final output file (.bin) from ELF output file.
|
||||
%.bin: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O binary $< $@
|
||||
|
||||
# Create extended listing file/disassambly from ELF output file.
|
||||
# using objdump testing: option -C
|
||||
%.lss: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_EXTENDED_LISTING) $@
|
||||
$(OBJDUMP) -h -S -C -r $< > $@
|
||||
# $(OBJDUMP) -x -S $< > $@
|
||||
|
||||
# Create a symbol table from ELF output file.
|
||||
%.sym: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_SYMBOL_TABLE) $@
|
||||
$(NM) -n $< > $@
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
.SECONDARY : $(TARGET).elf
|
||||
.PRECIOUS : $(ALLOBJ)
|
||||
%.elf: $(ALLOBJ)
|
||||
@echo $(MSG_LINKING) $@
|
||||
# use $(CC) for C-only projects or $(CPP) for C++-projects:
|
||||
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
define ASSEMBLE_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
define ASSEMBLE_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
define COMPILE_C_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
define COMPILE_C_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
define COMPILE_CPP_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
define COMPILE_CPP_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(SRC:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC) $< to $@
|
||||
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(SRCARM:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC_ARM) $< to $@
|
||||
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Target: clean project.
|
||||
clean: begin clean_list finished end
|
||||
|
||||
clean_list :
|
||||
## @echo
|
||||
@echo $(MSG_CLEANING)
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(REMOVE) $(wildcard $(OUTDIR)/*.c)
|
||||
$(REMOVE) $(wildcard $(OUTDIR)/*.h)
|
||||
$(REMOVE) $(ALLOBJ)
|
||||
$(REMOVE) $(LSTFILES)
|
||||
$(REMOVE) $(DEPFILES)
|
||||
$(REMOVE) $(SRC:.c=.s)
|
||||
$(REMOVE) $(SRCARM:.c=.s)
|
||||
$(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(OUTDIR) 2>NUL)
|
||||
else
|
||||
$(shell mkdir $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all begin finish end sizebefore sizeafter gccversion \
|
||||
build elf hex bin lss sym clean clean_list program gencode
|
||||
|
131
flight/targets/boards/sparky2/firmware/UAVObjects.inc
Normal file
131
flight/targets/boards/sparky2/firmware/UAVObjects.inc
Normal file
@ -0,0 +1,131 @@
|
||||
#
|
||||
# Copyright (c) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# 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
|
||||
#
|
||||
|
||||
# These are the UAVObjects supposed to be build as part of the target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += statusgrounddrive
|
||||
UAVOBJSRCFILENAMES += statusvtolautotakeoff
|
||||
UAVOBJSRCFILENAMES += pidstatus
|
||||
UAVOBJSRCFILENAMES += statusvtolland
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
UAVOBJSRCFILENAMES += actuatordesired
|
||||
UAVOBJSRCFILENAMES += actuatorsettings
|
||||
UAVOBJSRCFILENAMES += attitudesettings
|
||||
UAVOBJSRCFILENAMES += attitudestate
|
||||
UAVOBJSRCFILENAMES += gyrostate
|
||||
UAVOBJSRCFILENAMES += gyrosensor
|
||||
UAVOBJSRCFILENAMES += accelstate
|
||||
UAVOBJSRCFILENAMES += accelsensor
|
||||
UAVOBJSRCFILENAMES += magsensor
|
||||
UAVOBJSRCFILENAMES += auxmagsensor
|
||||
UAVOBJSRCFILENAMES += auxmagsettings
|
||||
UAVOBJSRCFILENAMES += magstate
|
||||
UAVOBJSRCFILENAMES += barosensor
|
||||
UAVOBJSRCFILENAMES += airspeedsensor
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedstate
|
||||
UAVOBJSRCFILENAMES += debuglogsettings
|
||||
UAVOBJSRCFILENAMES += debuglogcontrol
|
||||
UAVOBJSRCFILENAMES += debuglogstatus
|
||||
UAVOBJSRCFILENAMES += debuglogentry
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
UAVOBJSRCFILENAMES += flightbatterystate
|
||||
UAVOBJSRCFILENAMES += flightplancontrol
|
||||
UAVOBJSRCFILENAMES += flightplansettings
|
||||
UAVOBJSRCFILENAMES += flightplanstatus
|
||||
UAVOBJSRCFILENAMES += flighttelemetrystats
|
||||
UAVOBJSRCFILENAMES += gcstelemetrystats
|
||||
UAVOBJSRCFILENAMES += gcsreceiver
|
||||
UAVOBJSRCFILENAMES += gpspositionsensor
|
||||
UAVOBJSRCFILENAMES += gpssatellites
|
||||
UAVOBJSRCFILENAMES += gpstime
|
||||
UAVOBJSRCFILENAMES += gpsvelocitysensor
|
||||
UAVOBJSRCFILENAMES += gpssettings
|
||||
UAVOBJSRCFILENAMES += gpsextendedstatus
|
||||
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
|
||||
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
|
||||
UAVOBJSRCFILENAMES += vtolpathfollowersettings
|
||||
UAVOBJSRCFILENAMES += groundpathfollowersettings
|
||||
UAVOBJSRCFILENAMES += homelocation
|
||||
UAVOBJSRCFILENAMES += i2cstats
|
||||
UAVOBJSRCFILENAMES += manualcontrolcommand
|
||||
UAVOBJSRCFILENAMES += manualcontrolsettings
|
||||
UAVOBJSRCFILENAMES += flightmodesettings
|
||||
UAVOBJSRCFILENAMES += mixersettings
|
||||
UAVOBJSRCFILENAMES += mixerstatus
|
||||
UAVOBJSRCFILENAMES += nedaccel
|
||||
UAVOBJSRCFILENAMES += objectpersistence
|
||||
UAVOBJSRCFILENAMES += oplinkreceiver
|
||||
UAVOBJSRCFILENAMES += overosyncstats
|
||||
UAVOBJSRCFILENAMES += overosyncsettings
|
||||
UAVOBJSRCFILENAMES += pathaction
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += pathplan
|
||||
UAVOBJSRCFILENAMES += pathstatus
|
||||
UAVOBJSRCFILENAMES += pathsummary
|
||||
UAVOBJSRCFILENAMES += positionstate
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
UAVOBJSRCFILENAMES += revocalibration
|
||||
UAVOBJSRCFILENAMES += revosettings
|
||||
UAVOBJSRCFILENAMES += sonaraltitude
|
||||
UAVOBJSRCFILENAMES += stabilizationdesired
|
||||
UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
|
||||
UAVOBJSRCFILENAMES += stabilizationstatus
|
||||
UAVOBJSRCFILENAMES += stabilizationbank
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
UAVOBJSRCFILENAMES += systemstats
|
||||
UAVOBJSRCFILENAMES += taskinfo
|
||||
UAVOBJSRCFILENAMES += callbackinfo
|
||||
UAVOBJSRCFILENAMES += velocitystate
|
||||
UAVOBJSRCFILENAMES += velocitydesired
|
||||
UAVOBJSRCFILENAMES += watchdogstatus
|
||||
UAVOBJSRCFILENAMES += flightstatus
|
||||
UAVOBJSRCFILENAMES += hwsettings
|
||||
UAVOBJSRCFILENAMES += receiveractivity
|
||||
UAVOBJSRCFILENAMES += receiverstatus
|
||||
UAVOBJSRCFILENAMES += cameradesired
|
||||
UAVOBJSRCFILENAMES += camerastabsettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
UAVOBJSRCFILENAMES += oplinksettings
|
||||
UAVOBJSRCFILENAMES += oplinkstatus
|
||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += waypoint
|
||||
UAVOBJSRCFILENAMES += waypointactive
|
||||
UAVOBJSRCFILENAMES += poilocation
|
||||
UAVOBJSRCFILENAMES += poilearnsettings
|
||||
UAVOBJSRCFILENAMES += mpugyroaccelsettings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
90
flight/targets/boards/sparky2/firmware/cm3_fault_handlers.c
Normal file
90
flight/targets/boards/sparky2/firmware/cm3_fault_handlers.c
Normal file
@ -0,0 +1,90 @@
|
||||
/*
|
||||
* cm3_fault_handlers.c
|
||||
*
|
||||
* Created on: Apr 24, 2011
|
||||
* Author: msmith
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include "inc/dcc_stdio.h"
|
||||
#ifdef STM32F4XX
|
||||
# include <stm32f4xx.h>
|
||||
#endif
|
||||
#ifdef STM32F2XX
|
||||
# include <stm32f2xx.h>
|
||||
#endif
|
||||
|
||||
#define FAULT_TRAMPOLINE(_vec) \
|
||||
__attribute__((naked, no_instrument_function)) \
|
||||
void \
|
||||
_vec##_Handler(void) \
|
||||
{ \
|
||||
__asm(".syntax unified\n" \
|
||||
"MOVS R0, #4 \n" \
|
||||
"MOV R1, LR \n" \
|
||||
"TST R0, R1 \n" \
|
||||
"BEQ 1f \n" \
|
||||
"MRS R0, PSP \n" \
|
||||
"B " #_vec "_Handler2 \n" \
|
||||
"1: \n" \
|
||||
"MRS R0, MSP \n" \
|
||||
"B " #_vec "_Handler2 \n" \
|
||||
".syntax divided\n"); \
|
||||
} \
|
||||
struct hack
|
||||
|
||||
struct cm3_frame {
|
||||
uint32_t r0;
|
||||
uint32_t r1;
|
||||
uint32_t r2;
|
||||
uint32_t r3;
|
||||
uint32_t r12;
|
||||
uint32_t lr;
|
||||
uint32_t pc;
|
||||
uint32_t psr;
|
||||
};
|
||||
|
||||
FAULT_TRAMPOLINE(HardFault);
|
||||
FAULT_TRAMPOLINE(BusFault);
|
||||
FAULT_TRAMPOLINE(UsageFault);
|
||||
|
||||
/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */
|
||||
#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg))
|
||||
|
||||
void HardFault_Handler2(struct cm3_frame *frame)
|
||||
{
|
||||
dbg_write_str("\nHARD FAULT");
|
||||
dbg_write_hex32(frame->pc);
|
||||
dbg_write_char('\n');
|
||||
dbg_write_hex32(SCB_REG(HFSR));
|
||||
dbg_write_char('\n');
|
||||
for (;;) {
|
||||
;
|
||||
}
|
||||
}
|
||||
|
||||
void BusFault_Handler2(struct cm3_frame *frame)
|
||||
{
|
||||
dbg_write_str("\nBUS FAULT");
|
||||
dbg_write_hex32(frame->pc);
|
||||
dbg_write_char('\n');
|
||||
dbg_write_hex32(SCB_REG(CFSR));
|
||||
dbg_write_char('\n');
|
||||
dbg_write_hex32(SCB_REG(BFAR));
|
||||
dbg_write_char('\n');
|
||||
for (;;) {
|
||||
;
|
||||
}
|
||||
}
|
||||
|
||||
void UsageFault_Handler2(struct cm3_frame *frame)
|
||||
{
|
||||
dbg_write_str("\nUSAGE FAULT");
|
||||
dbg_write_hex32(frame->pc);
|
||||
dbg_write_char('\n');
|
||||
dbg_write_hex32(SCB_REG(CFSR));
|
||||
dbg_write_char('\n');
|
||||
for (;;) {
|
||||
;
|
||||
}
|
||||
}
|
149
flight/targets/boards/sparky2/firmware/dcc_stdio.c
Normal file
149
flight/targets/boards/sparky2/firmware/dcc_stdio.c
Normal file
@ -0,0 +1,149 @@
|
||||
/***************************************************************************
|
||||
* Copyright (C) 2008 by Dominic Rath *
|
||||
* Dominic.Rath@gmx.de *
|
||||
* Copyright (C) 2008 by Spencer Oliver *
|
||||
* spen@spen-soft.co.uk *
|
||||
* Copyright (C) 2008 by Frederik Kriewtz *
|
||||
* frederik@kriewitz.eu *
|
||||
* *
|
||||
* 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 2 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/dcc_stdio.h"
|
||||
|
||||
#define TARGET_REQ_TRACEMSG 0x00
|
||||
#define TARGET_REQ_DEBUGMSG_ASCII 0x01
|
||||
#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8))
|
||||
#define TARGET_REQ_DEBUGCHAR 0x02
|
||||
|
||||
/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel
|
||||
* DCRDR[7:0] is used by target for status
|
||||
* DCRDR[15:8] is used by target for write buffer
|
||||
* DCRDR[23:16] is used for by host for status
|
||||
* DCRDR[31:24] is used for by host for write buffer */
|
||||
|
||||
#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8))
|
||||
|
||||
#define BUSY 1
|
||||
|
||||
void dbg_write(unsigned long dcc_data)
|
||||
{
|
||||
int len = 4;
|
||||
|
||||
while (len--) {
|
||||
/* wait for data ready */
|
||||
while (NVIC_DBG_DATA_R & BUSY) {
|
||||
;
|
||||
}
|
||||
|
||||
/* write our data and set write flag - tell host there is data*/
|
||||
NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY);
|
||||
dcc_data >>= 8;
|
||||
}
|
||||
}
|
||||
|
||||
void dbg_trace_point(unsigned long number)
|
||||
{
|
||||
dbg_write(TARGET_REQ_TRACEMSG | (number << 8));
|
||||
}
|
||||
|
||||
void dbg_write_u32(const unsigned long *val, long len)
|
||||
{
|
||||
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16));
|
||||
|
||||
while (len > 0) {
|
||||
dbg_write(*val);
|
||||
|
||||
val++;
|
||||
len--;
|
||||
}
|
||||
}
|
||||
|
||||
void dbg_write_u16(const unsigned short *val, long len)
|
||||
{
|
||||
unsigned long dcc_data;
|
||||
|
||||
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16));
|
||||
|
||||
while (len > 0) {
|
||||
dcc_data = val[0]
|
||||
| ((len > 1) ? val[1] << 16 : 0x0000);
|
||||
|
||||
dbg_write(dcc_data);
|
||||
|
||||
val += 2;
|
||||
len -= 2;
|
||||
}
|
||||
}
|
||||
|
||||
void dbg_write_u8(const unsigned char *val, long len)
|
||||
{
|
||||
unsigned long dcc_data;
|
||||
|
||||
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16));
|
||||
|
||||
while (len > 0) {
|
||||
dcc_data = val[0]
|
||||
| ((len > 1) ? val[1] << 8 : 0x00)
|
||||
| ((len > 2) ? val[2] << 16 : 0x00)
|
||||
| ((len > 3) ? val[3] << 24 : 0x00);
|
||||
|
||||
dbg_write(dcc_data);
|
||||
|
||||
val += 4;
|
||||
len -= 4;
|
||||
}
|
||||
}
|
||||
|
||||
void dbg_write_str(const char *msg)
|
||||
{
|
||||
long len;
|
||||
unsigned long dcc_data;
|
||||
|
||||
for (len = 0; msg[len] && (len < 65536); len++) {
|
||||
;
|
||||
}
|
||||
|
||||
dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16));
|
||||
|
||||
while (len > 0) {
|
||||
dcc_data = msg[0]
|
||||
| ((len > 1) ? msg[1] << 8 : 0x00)
|
||||
| ((len > 2) ? msg[2] << 16 : 0x00)
|
||||
| ((len > 3) ? msg[3] << 24 : 0x00);
|
||||
dbg_write(dcc_data);
|
||||
|
||||
msg += 4;
|
||||
len -= 4;
|
||||
}
|
||||
}
|
||||
|
||||
void dbg_write_char(char msg)
|
||||
{
|
||||
dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16));
|
||||
}
|
||||
|
||||
void dbg_write_hex32(const unsigned long val)
|
||||
{
|
||||
static const char hextab[] = {
|
||||
'0', '1', '2', '3', '4', '5', '6', '7',
|
||||
'8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
|
||||
};
|
||||
|
||||
for (int shift = 28; shift >= 0; shift -= 4) {
|
||||
dbg_write_char(hextab[(val >> shift) & 0xf]);
|
||||
}
|
||||
}
|
104
flight/targets/boards/sparky2/firmware/inc/FreeRTOSConfig.h
Normal file
104
flight/targets/boards/sparky2/firmware/inc/FreeRTOSConfig.h
Normal file
@ -0,0 +1,104 @@
|
||||
#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 configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ...
|
||||
#define configTICK_RATE_HZ ((portTickType)1000)
|
||||
#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5)
|
||||
#define configMINIMAL_STACK_SIZE ((unsigned short)512)
|
||||
#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total
|
||||
#define configMAX_TASK_NAME_LEN (16)
|
||||
|
||||
#define configUSE_PREEMPTION 1
|
||||
#define configUSE_IDLE_HOOK 1
|
||||
#define configUSE_TICK_HOOK 0
|
||||
#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 configCHECK_FOR_STACK_OVERFLOW 2
|
||||
#define configQUEUE_REGISTRY_SIZE 10
|
||||
|
||||
#define configUSE_TIMERS 1
|
||||
#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */
|
||||
#define configTIMER_QUEUE_LENGTH 10
|
||||
#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE
|
||||
|
||||
/* 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 1
|
||||
#define INCLUDE_vTaskSuspend 1
|
||||
#define INCLUDE_vTaskDelayUntil 1
|
||||
#define INCLUDE_vTaskDelay 1
|
||||
#define INCLUDE_xTaskGetSchedulerState 1
|
||||
#define INCLUDE_xTaskGetCurrentTaskHandle 1
|
||||
#define INCLUDE_uxTaskGetStackHighWaterMark 1
|
||||
#define INCLUDE_xTaskGetIdleTaskHandle 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
|
||||
|
||||
/* Enable run time stats collection */
|
||||
#define configGENERATE_RUN_TIME_STATS 1
|
||||
#define INCLUDE_uxTaskGetRunTime 1
|
||||
|
||||
/*
|
||||
* Once we move to CMSIS2 we can at least use:
|
||||
*
|
||||
* CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
|
||||
*
|
||||
* (still nothing for the DWT registers, surprisingly)
|
||||
*/
|
||||
#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 */
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
#endif /* FREERTOS_CONFIG_H */
|
36
flight/targets/boards/sparky2/firmware/inc/dcc_stdio.h
Normal file
36
flight/targets/boards/sparky2/firmware/inc/dcc_stdio.h
Normal file
@ -0,0 +1,36 @@
|
||||
/***************************************************************************
|
||||
* Copyright (C) 2008 by Dominic Rath *
|
||||
* Dominic.Rath@gmx.de *
|
||||
* Copyright (C) 2008 by Spencer Oliver *
|
||||
* spen@spen-soft.co.uk *
|
||||
* *
|
||||
* 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 2 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 DCC_STDIO_H
|
||||
#define DCC_STDIO_H
|
||||
|
||||
void dbg_trace_point(unsigned long number);
|
||||
|
||||
void dbg_write_u32(const unsigned long *val, long len);
|
||||
void dbg_write_u16(const unsigned short *val, long len);
|
||||
void dbg_write_u8(const unsigned char *val, long len);
|
||||
|
||||
void dbg_write_str(const char *msg);
|
||||
void dbg_write_char(char msg);
|
||||
void dbg_write_hex32(const unsigned long val);
|
||||
|
||||
#endif /* DCC_STDIO_H */
|
52
flight/targets/boards/sparky2/firmware/inc/openpilot.h
Normal file
52
flight/targets/boards/sparky2/firmware/inc/openpilot.h
Normal file
@ -0,0 +1,52 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file openpilot.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Main OpenPilot header.
|
||||
* @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 OPENPILOT_H
|
||||
#define OPENPILOT_H
|
||||
|
||||
/* PIOS Includes */
|
||||
#include <pios.h>
|
||||
|
||||
/* OpenPilot Libraries */
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
#endif /* OPENPILOT_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
82
flight/targets/boards/sparky2/firmware/inc/pios_board_sim.h
Normal file
82
flight/targets/boards/sparky2/firmware/inc/pios_board_sim.h
Normal file
@ -0,0 +1,82 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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_ALARM 0
|
||||
#define PIOS_LED_HEARTBEAT 1
|
||||
#define PIOS_LED_NUM 2
|
||||
|
||||
// -------------------------
|
||||
// COM
|
||||
//
|
||||
// See also pios_board_posix.c
|
||||
// -------------------------
|
||||
// #define PIOS_USART_TX_BUFFER_SIZE 256
|
||||
#define PIOS_COM_BUFFER_SIZE 1024
|
||||
#define PIOS_COM_MAX_DEVS 255
|
||||
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
|
||||
#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
|
||||
|
||||
extern uint32_t pios_com_telem_rf_id;
|
||||
extern uint32_t pios_com_telem_usb_id;
|
||||
extern uint32_t pios_com_gps_id;
|
||||
extern uint32_t pios_com_aux_id;
|
||||
extern uint32_t pios_com_spectrum_id;
|
||||
|
||||
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
|
||||
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
|
||||
#define PIOS_COM_GPS (pios_com_gps_id)
|
||||
|
||||
#ifdef PIOS_ENABLE_AUX_UART
|
||||
#define PIOS_COM_AUX (pios_com_aux_id)
|
||||
#define PIOS_COM_DEBUG (PIOS_COM_AUX)
|
||||
#endif
|
||||
|
||||
#define PIOS_GCSRCVR_TIMEOUT_MS 200
|
||||
/**
|
||||
* 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 */
|
197
flight/targets/boards/sparky2/firmware/inc/pios_config.h
Normal file
197
flight/targets/boards/sparky2/firmware/inc/pios_config.h
Normal file
@ -0,0 +1,197 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* 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 Callback Scheduler 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 10
|
||||
|
||||
/* 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 */
|
||||
/* seems to be completely unused #define PIOS_MPU6000_ACCEL */
|
||||
/* #define PIOS_INCLUDE_HMC5843 */
|
||||
#define PIOS_INCLUDE_HMC5X83
|
||||
/* Sparky2 5X83s are all external and thus don't have GPIOs #define PIOS_HMC5X83_HAS_GPIOS */
|
||||
/* #define PIOS_INCLUDE_BMP085 */
|
||||
#define PIOS_INCLUDE_MS5611
|
||||
#define PIOS_INCLUDE_MPXV
|
||||
#define PIOS_INCLUDE_ETASV3
|
||||
#define PIOS_INCLUDE_MS4525DO
|
||||
#define PIOS_INCLUDE_MPU9250
|
||||
#define PIOS_MPU9250_ACCEL
|
||||
#define PIOS_MPU9250_MAG
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
#define PIOS_INCLUDE_WS2811
|
||||
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
/* 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_SRXL
|
||||
#define PIOS_INCLUDE_HOTT
|
||||
#define PIOS_INCLUDE_EXBUS
|
||||
#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_INTERNAL
|
||||
#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS
|
||||
/* #define PIOS_INCLUDE_FLASH_OBJLIST */
|
||||
/* #define PIOS_INCLUDE_FLASH_EEPROM */
|
||||
#define FLASH_FREERTOS
|
||||
|
||||
#define PIOS_INCLUDE_DEBUGLOG
|
||||
|
||||
/* 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_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
#define PIOS_QUATERNION_STABILIZATION
|
||||
|
||||
/* Performance counters */
|
||||
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692
|
||||
|
||||
/* Alarm Thresholds */
|
||||
#define HEAP_LIMIT_WARNING 1000
|
||||
#define HEAP_LIMIT_CRITICAL 500
|
||||
#define IRQSTACK_LIMIT_WARNING 150
|
||||
#define IRQSTACK_LIMIT_CRITICAL 80
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
/* Task stack sizes */
|
||||
/* #define PIOS_ACTUATOR_STACK_SIZE 1020 */
|
||||
/* #define PIOS_MANUAL_STACK_SIZE 800 */
|
||||
#define PIOS_SYSTEM_STACK_SIZE 1536
|
||||
/* #define PIOS_STABILIZATION_STACK_SIZE 524 */
|
||||
/* #define PIOS_TELEM_STACK_SIZE 500 */
|
||||
/* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */
|
||||
|
||||
/* This can't be too high to stop eventdispatcher thread overflowing */
|
||||
#define PIOS_EVENTDISAPTCHER_QUEUE 10
|
||||
|
||||
/* Revolution series */
|
||||
#define REVOLUTION
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
81
flight/targets/boards/sparky2/firmware/inc/pios_config_sim.h
Normal file
81
flight/targets/boards/sparky2/firmware/inc/pios_config_sim.h
Normal file
@ -0,0 +1,81 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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_SDCARD
|
||||
#define PIOS_INCLUDE_FREERTOS
|
||||
#define PIOS_INCLUDE_CALLBACKSCHEDULER
|
||||
#define PIOS_INCLUDE_TASK_MONITOR
|
||||
#define PIOS_INCLUDE_COM
|
||||
// #define PIOS_INCLUDE_GPS
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_TELEMETRY_RF
|
||||
#define PIOS_INCLUDE_TCP
|
||||
#define PIOS_INCLUDE_UDP
|
||||
#define PIOS_INCLUDE_SERVO
|
||||
#define PIOS_INCLUDE_RCVR
|
||||
#define PIOS_INCLUDE_GCSRCVR
|
||||
|
||||
#define PIOS_RCVR_MAX_CHANNELS 12
|
||||
#define PIOS_RCVR_MAX_DEVS 3
|
||||
|
||||
/* Defaults for Logging */
|
||||
#define LOG_FILENAME "PIOS.LOG"
|
||||
#define STARTUP_LOG_ENABLED 1
|
||||
|
||||
/* COM Module */
|
||||
#define GPS_BAUDRATE 19200
|
||||
#define TELEM_BAUDRATE 19200
|
||||
#define AUXUART_ENABLED 0
|
||||
#define AUXUART_BAUDRATE 19200
|
||||
|
||||
#define TELEM_QUEUE_SIZE 20
|
||||
#define PIOS_TELEM_STACK_SIZE 2048
|
||||
|
||||
/* Stabilization options */
|
||||
#define PIOS_QUATERNION_STABILIZATION
|
||||
|
||||
/* GPS options */
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
#define HEAP_LIMIT_WARNING 4000
|
||||
#define HEAP_LIMIT_CRITICAL 1000
|
||||
#define IRQSTACK_LIMIT_WARNING 150
|
||||
#define IRQSTACK_LIMIT_CRITICAL 80
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
#define REVOLUTION
|
||||
|
||||
#endif /* PIOS_CONFIG_POSIX_H */
|
@ -0,0 +1,47 @@
|
||||
/**
|
||||
***********************************************************************************
|
||||
* @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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @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 <pios_usb_defs.h> /* USB_* macros */
|
||||
|
||||
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_SPARKY2
|
||||
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_SPARKY2, USB_OP_BOARD_MODE_FW)
|
||||
#define PIOS_USB_BOARD_SN_SUFFIX "+FW"
|
||||
|
||||
#endif /* PIOS_USB_BOARD_DATA_H */
|
1101
flight/targets/boards/sparky2/firmware/pios_board.c
Normal file
1101
flight/targets/boards/sparky2/firmware/pios_board.c
Normal file
@ -0,0 +1,1101 @@
|
||||
/**
|
||||
****************************************************************************************
|
||||
* @file pios_board.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* 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 Sparky2 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 <pios_board_info.h>
|
||||
#include <uavobjectsinit.h>
|
||||
#include <hwsettings.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <oplinksettings.h>
|
||||
#include <oplinkstatus.h>
|
||||
#include <oplinkreceiver.h>
|
||||
#include <pios_oplinkrcvr_priv.h>
|
||||
#include <taskinfo.h>
|
||||
#include <pios_ws2811.h>
|
||||
#include <sanitycheck.h>
|
||||
#include <actuatorsettings.h>
|
||||
#include <auxmagsettings.h>
|
||||
|
||||
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
||||
#include <pios_instrumentation.h>
|
||||
#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"
|
||||
|
||||
/**
|
||||
* Sensor configurations
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
|
||||
#include "pios_adc_priv.h"
|
||||
void PIOS_ADC_DMC_irq_handler(void);
|
||||
void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
|
||||
struct pios_adc_cfg pios_adc_cfg = {
|
||||
.adc_dev = ADC1,
|
||||
.dma = {
|
||||
.irq = {
|
||||
.flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA2_Stream4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.channel = DMA2_Stream4,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_0,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR
|
||||
},
|
||||
}
|
||||
},
|
||||
.half_flag = DMA_IT_HTIF4,
|
||||
.full_flag = DMA_IT_TCIF4,
|
||||
};
|
||||
void PIOS_ADC_DMC_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_ADC_DMA_Handler();
|
||||
}
|
||||
|
||||
#endif /* if defined(PIOS_INCLUDE_ADC) */
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
#include "pios_hmc5x83.h"
|
||||
pios_hmc5x83_dev_t i2c_port_mag = 0;
|
||||
pios_hmc5x83_dev_t flexi_port_mag = 0;
|
||||
|
||||
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
|
||||
#ifdef PIOS_HMC5X83_HAS_GPIOS
|
||||
.exti_cfg = NULL,
|
||||
#endif /* PIOS_HMC5X83_HAS_GPIOS */
|
||||
.M_ODR = PIOS_HMC5x83_ODR_75,
|
||||
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
|
||||
.Gain = PIOS_HMC5x83_GAIN_1_9,
|
||||
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
|
||||
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
|
||||
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
|
||||
/**
|
||||
* Configuration for the MS5611 chip
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_MS5611)
|
||||
#include "pios_ms5611.h"
|
||||
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
|
||||
.oversampling = MS5611_OSR_4096,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_MS5611 */
|
||||
|
||||
/**
|
||||
* Configuration for the MPU9250 chip
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_MPU9250)
|
||||
#include "pios_mpu9250.h"
|
||||
#include "pios_mpu9250_config.h"
|
||||
static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = {
|
||||
.vector = PIOS_MPU9250_IRQHandler,
|
||||
.line = EXTI_Line5,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI9_5_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.exti = {
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line5, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Rising,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_mpu9250_cfg pios_mpu9250_cfg = {
|
||||
.exti_cfg = &pios_exti_mpu9250_cfg,
|
||||
.Fifo_store = 0,
|
||||
// Clock at 8 khz
|
||||
.Smpl_rate_div_no_dlp = 0,
|
||||
// with dlp on output rate is 1000Hz
|
||||
.Smpl_rate_div_dlp = 0,
|
||||
.interrupt_cfg = PIOS_MPU9250_INT_CLR_ANYRD, // | PIOS_MPU9250_INT_LATCH_EN,
|
||||
.interrupt_en = PIOS_MPU9250_INTEN_DATA_RDY,
|
||||
.User_ctl = PIOS_MPU9250_USERCTL_DIS_I2C | PIOS_MPU9250_USERCTL_I2C_MST_EN,
|
||||
.Pwr_mgmt_clk = PIOS_MPU9250_PWRMGMT_PLL_Z_CLK,
|
||||
.accel_range = PIOS_MPU9250_ACCEL_8G,
|
||||
.gyro_range = PIOS_MPU9250_SCALE_2000_DEG,
|
||||
.filter = PIOS_MPU9250_LOWPASS_256_HZ,
|
||||
.orientation = PIOS_MPU9250_TOP_180DEG,
|
||||
.fast_prescaler = PIOS_SPI_PRESCALER_4,
|
||||
.std_prescaler = PIOS_SPI_PRESCALER_64,
|
||||
.max_downsample = 26,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_MPU9250 */
|
||||
|
||||
|
||||
/* One slot per selectable receiver group.
|
||||
* eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS
|
||||
* NOTE: No slot in this map for NONE.
|
||||
*/
|
||||
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
|
||||
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 128
|
||||
#define PIOS_COM_GPS_TX_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_RFM22B_RF_RX_BUF_LEN 512
|
||||
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
|
||||
|
||||
#define PIOS_COM_HKOSD_RX_BUF_LEN 22
|
||||
#define PIOS_COM_HKOSD_TX_BUF_LEN 22
|
||||
|
||||
#define PIOS_COM_MSP_TX_BUF_LEN 128
|
||||
#define PIOS_COM_MSP_RX_BUF_LEN 64
|
||||
|
||||
#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_gps_id = 0;
|
||||
uint32_t pios_com_telem_usb_id = 0;
|
||||
uint32_t pios_com_telem_rf_id = 0;
|
||||
uint32_t pios_com_rf_id = 0;
|
||||
uint32_t pios_com_bridge_id = 0;
|
||||
uint32_t pios_com_overo_id = 0;
|
||||
uint32_t pios_com_hkosd_id = 0;
|
||||
uint32_t pios_com_msp_id = 0;
|
||||
|
||||
uint32_t pios_com_vcp_id = 0;
|
||||
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
uint32_t pios_rfm22b_id = 0;
|
||||
#endif
|
||||
|
||||
uintptr_t pios_uavo_settings_fs_id;
|
||||
uintptr_t pios_user_fs_id;
|
||||
|
||||
/*
|
||||
* Setup a com port based on the passed cfg, driver and buffer sizes.
|
||||
* tx size of -1 make the port rx only
|
||||
* rx size of -1 make the port tx only
|
||||
* having both tx and rx size of -1 is not valid and will fail further down in PIOS_COM_Init()
|
||||
*/
|
||||
static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len,
|
||||
const struct pios_com_driver *com_driver, uint32_t *pios_com_id)
|
||||
{
|
||||
uint32_t pios_usart_id;
|
||||
|
||||
if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint8_t *rx_buffer = 0, *tx_buffer = 0;
|
||||
|
||||
if (rx_buf_len > 0) {
|
||||
rx_buffer = (uint8_t *)pios_malloc(rx_buf_len);
|
||||
PIOS_Assert(rx_buffer);
|
||||
}
|
||||
|
||||
if (tx_buf_len > 0) {
|
||||
tx_buffer = (uint8_t *)pios_malloc(tx_buf_len);
|
||||
PIOS_Assert(tx_buffer);
|
||||
}
|
||||
|
||||
if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id,
|
||||
rx_buffer, rx_buf_len,
|
||||
tx_buffer, tx_buf_len)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
|
||||
const struct pios_com_driver *usart_com_driver,
|
||||
ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind)
|
||||
{
|
||||
uint32_t pios_usart_dsm_id;
|
||||
|
||||
if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_dsm_id;
|
||||
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver,
|
||||
pios_usart_dsm_id, *bind)) {
|
||||
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[channelgroup] = pios_dsm_rcvr_id;
|
||||
}
|
||||
|
||||
static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg)
|
||||
{
|
||||
uint32_t pios_ppm_id;
|
||||
|
||||
PIOS_PPM_Init(&pios_ppm_id, 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;
|
||||
}
|
||||
|
||||
static void PIOS_Board_configure_srxl(const struct pios_usart_cfg *usart_cfg)
|
||||
{
|
||||
uint32_t pios_usart_srxl_id;
|
||||
|
||||
if (PIOS_USART_Init(&pios_usart_srxl_id, usart_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_srxl_id;
|
||||
if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_srxl_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id;
|
||||
}
|
||||
|
||||
|
||||
static void PIOS_Board_configure_exbus(const struct pios_usart_cfg *usart_cfg)
|
||||
{
|
||||
uint32_t pios_usart_exbus_id;
|
||||
|
||||
if (PIOS_USART_Init(&pios_usart_exbus_id, usart_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_exbus_id;
|
||||
if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_exbus_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id;
|
||||
}
|
||||
|
||||
static void PIOS_Board_configure_hott(const struct pios_usart_cfg *usart_cfg, enum pios_hott_proto proto)
|
||||
{
|
||||
uint32_t pios_usart_hott_id;
|
||||
|
||||
if (PIOS_USART_Init(&pios_usart_hott_id, usart_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_hott_id;
|
||||
if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, proto)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_hott_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id;
|
||||
}
|
||||
|
||||
static void PIOS_Board_PPM_callback(const int16_t *channels)
|
||||
{
|
||||
uint8_t max_chan = (RFM22B_PPM_NUM_CHANNELS < OPLINKRECEIVER_CHANNEL_NUMELEM) ? RFM22B_PPM_NUM_CHANNELS : OPLINKRECEIVER_CHANNEL_NUMELEM;
|
||||
OPLinkReceiverData opl_rcvr;
|
||||
|
||||
for (uint8_t i = 0; i < max_chan; ++i) {
|
||||
opl_rcvr.Channel[i] = channels[i];
|
||||
}
|
||||
OPLinkReceiverSet(&opl_rcvr);
|
||||
}
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
|
||||
#include <pios_board_info.h>
|
||||
|
||||
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
|
||||
|
||||
/* Set up the SPI interface to the gyro/acelerometer */
|
||||
if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
/* Set up the SPI interface to the flash and rfm22b */
|
||||
if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
#ifdef PIOS_INCLUDE_I2C
|
||||
if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_FLASH)
|
||||
/* Connect flash to the appropriate interface and configure it */
|
||||
uintptr_t flash_id = 0;
|
||||
|
||||
// Initialize the external USER flash
|
||||
if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_external_system_cfg, &pios_jedec_flash_driver, flash_id)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_FLASH) */
|
||||
|
||||
#if defined(PIOS_INCLUDE_RTC)
|
||||
PIOS_RTC_Init(&pios_rtc_main_cfg);
|
||||
#endif
|
||||
/* IAP System Setup */
|
||||
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);
|
||||
}
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_Init();
|
||||
#endif
|
||||
|
||||
/* 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();
|
||||
HwSettingsInitialize();
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
OPLinkSettingsInitialize();
|
||||
OPLinkStatusInitialize();
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
AuxMagSettingsInitialize();
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
/* 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);
|
||||
PIOS_TIM_InitClock(&tim_5_cfg);
|
||||
PIOS_TIM_InitClock(&tim_8_cfg);
|
||||
PIOS_TIM_InitClock(&tim_9_cfg);
|
||||
PIOS_TIM_InitClock(&tim_10_cfg);
|
||||
PIOS_TIM_InitClock(&tim_11_cfg);
|
||||
PIOS_TIM_InitClock(&tim_12_cfg);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/* Configure IO ports */
|
||||
uint8_t hwsettings_DSMxBind;
|
||||
HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
|
||||
|
||||
/* Configure FlexiPort */
|
||||
uint8_t hwsettings_flexiport;
|
||||
HwSettingsSPK2_FlexiPortGet(&hwsettings_flexiport);
|
||||
switch (hwsettings_flexiport) {
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_TELEMETRY:
|
||||
PIOS_Board_configure_com(&pios_usart_flexi_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_SPK2_FLEXIPORT_I2C:
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
{
|
||||
// get auxmag type
|
||||
AuxMagSettingsTypeOptions option;
|
||||
AuxMagSettingsTypeGet(&option);
|
||||
// the FlexiPort type is I2C, so if the AuxMag type is Flexi(Port) then set it up
|
||||
if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
|
||||
if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
|
||||
// to avoid making something else fail when HMC5X83 is removed
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
// attach the 5x83 mag to the previously inited I2C2
|
||||
flexi_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_flexiport_adapter_id, 0);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
// add this sensor to the sensor task's list
|
||||
// be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
|
||||
// and before registering some other fast and important sensor
|
||||
// as that would cause delay and time jitter for the second fast sensor
|
||||
PIOS_HMC5x83_Register(flexi_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
|
||||
// mag alarm is cleared later, so use I2C
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (flexi_port_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_GPS:
|
||||
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_DSM:
|
||||
// TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here
|
||||
PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg,
|
||||
&pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_DEBUGCONSOLE:
|
||||
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
|
||||
{
|
||||
PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_COMBRIDGE:
|
||||
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_MSP:
|
||||
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_OSDHK:
|
||||
PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_SRXL:
|
||||
#if defined(PIOS_INCLUDE_SRXL)
|
||||
PIOS_Board_configure_srxl(&pios_usart_srxl_flexi_cfg);
|
||||
#endif /* PIOS_INCLUDE_SRXL */
|
||||
break;
|
||||
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMD:
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMH:
|
||||
#if defined(PIOS_INCLUDE_HOTT)
|
||||
PIOS_Board_configure_hott(&pios_usart_hott_flexi_cfg,
|
||||
hwsettings_flexiport == HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH);
|
||||
#endif /* PIOS_INCLUDE_HOTT */
|
||||
break;
|
||||
|
||||
case HWSETTINGS_SPK2_FLEXIPORT_EXBUS:
|
||||
#if defined(PIOS_INCLUDE_EXBUS)
|
||||
PIOS_Board_configure_exbus(&pios_usart_exbus_flexi_cfg);
|
||||
#endif /* PIOS_INCLUDE_EXBUS */
|
||||
break;
|
||||
} /* hwsettings_spk2_flexiport */
|
||||
|
||||
/* Moved this here to allow binding on flexiport */
|
||||
#if defined(PIOS_INCLUDE_FLASH)
|
||||
if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_user_cfg, &pios_jedec_flash_driver, flash_id)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_FLASH) */
|
||||
|
||||
#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;
|
||||
PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
|
||||
|
||||
#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;
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
switch (hwsettings_usb_vcpport) {
|
||||
case HWSETTINGS_USB_VCPPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB_HID */
|
||||
|
||||
if (usb_hid_present || usb_cdc_present) {
|
||||
PIOS_USBHOOK_Activate();
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
|
||||
/* Configure main USART port */
|
||||
uint8_t hwsettings_mainport;
|
||||
HwSettingsSPK2_MainPortGet(&hwsettings_mainport);
|
||||
switch (hwsettings_mainport) {
|
||||
case HWSETTINGS_SPK2_MAINPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_TELEMETRY:
|
||||
PIOS_Board_configure_com(&pios_usart_main_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_SPK2_MAINPORT_GPS:
|
||||
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_DSM:
|
||||
// Force binding to zero on the main port
|
||||
hwsettings_DSMxBind = 0;
|
||||
|
||||
// TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here
|
||||
PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg,
|
||||
&pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_DEBUGCONSOLE:
|
||||
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
|
||||
{
|
||||
PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_COMBRIDGE:
|
||||
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_MSP:
|
||||
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id);
|
||||
break;
|
||||
case HWSETTINGS_SPK2_MAINPORT_OSDHK:
|
||||
PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
|
||||
break;
|
||||
} /* hwsettings_spk2_mainport */
|
||||
|
||||
|
||||
/* Initalize the RFM22B radio COM device. */
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
|
||||
/* Fetch the OPinkSettings object. */
|
||||
OPLinkSettingsData oplinkSettings;
|
||||
OPLinkSettingsGet(&oplinkSettings);
|
||||
|
||||
// Initialize out status object.
|
||||
OPLinkStatusData oplinkStatus;
|
||||
OPLinkStatusGet(&oplinkStatus);
|
||||
oplinkStatus.BoardType = bdinfo->board_type;
|
||||
PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM);
|
||||
PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial);
|
||||
oplinkStatus.BoardRevision = bdinfo->board_rev;
|
||||
|
||||
/* Is the radio turned on? */
|
||||
bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
|
||||
bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE);
|
||||
bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE);
|
||||
bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE);
|
||||
if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) {
|
||||
/* Configure the RFM22B device. */
|
||||
const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
|
||||
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
/* Configure the radio com interface */
|
||||
uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
|
||||
uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
|
||||
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
|
||||
|
||||
// Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.
|
||||
enum rfm22b_datarate datarate = RFM22_datarate_64000;
|
||||
switch (oplinkSettings.ComSpeed) {
|
||||
case OPLINKSETTINGS_COMSPEED_4800:
|
||||
datarate = RFM22_datarate_9600;
|
||||
break;
|
||||
case OPLINKSETTINGS_COMSPEED_9600:
|
||||
datarate = RFM22_datarate_19200;
|
||||
break;
|
||||
case OPLINKSETTINGS_COMSPEED_19200:
|
||||
datarate = RFM22_datarate_32000;
|
||||
break;
|
||||
case OPLINKSETTINGS_COMSPEED_38400:
|
||||
datarate = RFM22_datarate_64000;
|
||||
break;
|
||||
case OPLINKSETTINGS_COMSPEED_57600:
|
||||
datarate = RFM22_datarate_100000;
|
||||
break;
|
||||
case OPLINKSETTINGS_COMSPEED_115200:
|
||||
datarate = RFM22_datarate_192000;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Set the radio configuration parameters. */
|
||||
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
|
||||
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
|
||||
|
||||
/* Set the PPM callback if we should be receiving PPM. */
|
||||
if (ppm_mode || (ppm_only && !is_coordinator)) {
|
||||
PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
|
||||
}
|
||||
|
||||
/* Set the modem Tx poer level */
|
||||
switch (oplinkSettings.MaxRFPower) {
|
||||
case OPLINKSETTINGS_MAXRFPOWER_125:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_16:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_316:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_63:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_126:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_25:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_50:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
|
||||
break;
|
||||
case OPLINKSETTINGS_MAXRFPOWER_100:
|
||||
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
|
||||
break;
|
||||
default:
|
||||
// do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
/* Reinitialize the modem. */
|
||||
PIOS_RFM22B_Reinit(pios_rfm22b_id);
|
||||
} else {
|
||||
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
|
||||
}
|
||||
|
||||
OPLinkStatusSet(&oplinkStatus);
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
#if 1
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
const struct pios_servo_cfg *pios_servo_cfg;
|
||||
// default to servo outputs only
|
||||
pios_servo_cfg = &pios_servo_cfg_out;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// Configure the receiver port
|
||||
// Sparky2 receiver input on PC7 TIM8 CH2
|
||||
// include PPM,S.Bus,DSM,SRXL,EX.Bus,HoTT SUMD,HoTT SUMH
|
||||
uint8_t hwsettings_rcvrport;
|
||||
HwSettingsSPK2_RcvrPortGet(&hwsettings_rcvrport);
|
||||
|
||||
switch (hwsettings_rcvrport) {
|
||||
case HWSETTINGS_SPK2_RCVRPORT_PPM:
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_Board_configure_ppm(&pios_ppm_cfg);
|
||||
#endif /* PIOS_INCLUDE_PPM */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_RCVRPORT_SBUS:
|
||||
#if defined(PIOS_INCLUDE_SBUS)
|
||||
{
|
||||
uint32_t pios_usart_sbus_id;
|
||||
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_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
|
||||
break;
|
||||
case HWSETTINGS_SPK2_RCVRPORT_SRXL:
|
||||
#if defined(PIOS_INCLUDE_SRXL)
|
||||
PIOS_Board_configure_srxl(&pios_usart_srxl_rcvr_cfg);
|
||||
#endif /* PIOS_INCLUDE_SRXL */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD:
|
||||
case HWSETTINGS_SPK2_RCVRPORT_HOTTSUMH:
|
||||
#if defined(PIOS_INCLUDE_HOTT)
|
||||
PIOS_Board_configure_hott(&pios_usart_hott_rcvr_cfg,
|
||||
hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH);
|
||||
#endif /* PIOS_INCLUDE_HOTT */
|
||||
break;
|
||||
|
||||
case HWSETTINGS_SPK2_RCVRPORT_EXBUS:
|
||||
#if defined(PIOS_INCLUDE_EXBUS)
|
||||
PIOS_Board_configure_exbus(&pios_usart_exbus_rcvr_cfg);
|
||||
#endif /* PIOS_INCLUDE_EXBUS */
|
||||
break;
|
||||
case HWSETTINGS_SPK2_RCVRPORT_DSM:
|
||||
// TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here
|
||||
PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg,
|
||||
&pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT, &hwsettings_DSMxBind);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (hwsettings_rcvrport != HWSETTINGS_SPK2_RCVRPORT_SBUS) {
|
||||
GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init);
|
||||
GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable);
|
||||
}
|
||||
|
||||
#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 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_OPLINKRCVR)
|
||||
{
|
||||
OPLinkReceiverInitialize();
|
||||
uint32_t pios_oplinkrcvr_id;
|
||||
PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id);
|
||||
uint32_t pios_oplinkrcvr_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_OPLINKRCVR */
|
||||
|
||||
#if 1
|
||||
#ifndef PIOS_ENABLE_DEBUG_PINS
|
||||
// pios_servo_cfg points to the correct configuration based on input port settings
|
||||
PIOS_Servo_Init(pios_servo_cfg);
|
||||
#else
|
||||
PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins));
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout
|
||||
GPIO_InitTypeDef gpioA8 = {
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
};
|
||||
GPIO_Init(GPIOA, &gpioA8);
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
PIOS_ADC_Init(&pios_adc_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU9250)
|
||||
PIOS_MPU9250_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg);
|
||||
PIOS_MPU9250_CONFIG_Configure();
|
||||
PIOS_MPU9250_MainRegister();
|
||||
PIOS_MPU9250_MagRegister();
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
|
||||
// to avoid making something else fail when HMC5X83 is removed
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
// on board mag is in the MPU9250
|
||||
// this hmc5x83 mag is an external mag
|
||||
i2c_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
PIOS_HMC5x83_Register(i2c_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
|
||||
#else /* if 0 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
|
||||
// to avoid making something else fail when HMC5X83 is removed
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
// get auxmag type
|
||||
HwSettingsSPK2_I2CPortOptions i2cOption;
|
||||
AuxMagSettingsTypeOptions option;
|
||||
HwSettingsSPK2_I2CPortGet(&i2cOption);
|
||||
AuxMagSettingsTypeGet(&option);
|
||||
// if the I2CPort type is I2C(Port) and the AuxMag type is I2C(Port) then set it up
|
||||
if (i2cOption == HWSETTINGS_SPK2_I2CPORT_I2C && option == AUXMAGSETTINGS_TYPE_I2C) {
|
||||
// attach the 5x83 mag to the previously inited I2C2
|
||||
i2c_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
|
||||
// this is not in a loop, so it is safe
|
||||
PIOS_WDG_Clear();
|
||||
#endif /* PIOS_INCLUDE_WDG */
|
||||
// add this sensor to the sensor task's list
|
||||
// be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
|
||||
// and before registering some other fast and important sensor
|
||||
// as that would cause delay and time jitter for the second fast sensor
|
||||
PIOS_HMC5x83_Register(i2c_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
|
||||
// mag alarm is cleared later, so use I2C
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (i2c_port_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
#endif /* 0 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_MS5611)
|
||||
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
|
||||
PIOS_MS5611_Register();
|
||||
#endif /* PIOS_INCLUDE_MS5611 */
|
||||
|
||||
#ifdef PIOS_INCLUDE_WS2811
|
||||
#include <pios_ws2811.h>
|
||||
HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
|
||||
HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);
|
||||
|
||||
if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) {
|
||||
PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]);
|
||||
}
|
||||
#endif // PIOS_INCLUDE_WS2811
|
||||
#ifdef PIOS_INCLUDE_ADC
|
||||
{
|
||||
uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
|
||||
HwSettingsADCRoutingArrayGet(adc_config);
|
||||
for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
|
||||
if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
|
||||
PIOS_ADC_PinSetup(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // PIOS_INCLUDE_ADC
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
227
flight/targets/boards/sparky2/firmware/pios_board_sim.c
Normal file
227
flight/targets/boards/sparky2/firmware/pios_board_sim.c
Normal file
@ -0,0 +1,227 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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 <pios_com_priv.h>
|
||||
#include <pios_tcp_priv.h>
|
||||
#include <pios_udp_priv.h>
|
||||
#include <pios_rcvr_priv.h>
|
||||
#include <pios_gcsrcvr_priv.h>
|
||||
#include <uavobjectsinit.h>
|
||||
|
||||
#include <accelssensor.h>
|
||||
#include <barosensor.h>
|
||||
#include <gpspositionsensor.h>
|
||||
#include <gyrosensor.h>
|
||||
#include <magsensor.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
|
||||
void Stack_Change() {}
|
||||
|
||||
void Stack_Change_Weak() {}
|
||||
|
||||
|
||||
const struct pios_tcp_cfg pios_tcp_telem_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9001,
|
||||
};
|
||||
|
||||
const struct pios_udp_cfg pios_udp_telem_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9001,
|
||||
};
|
||||
|
||||
const struct pios_tcp_cfg pios_tcp_gps_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9001,
|
||||
};
|
||||
const struct pios_tcp_cfg pios_tcp_debug_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9002,
|
||||
};
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
const struct pios_tcp_cfg pios_tcp_aux_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9003,
|
||||
};
|
||||
#endif
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 96
|
||||
|
||||
/*
|
||||
* 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;
|
||||
extern const struct pios_com_driver pios_tcp_com_driver;
|
||||
|
||||
uint32_t pios_com_telem_rf_id;
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
uint32_t pios_com_gps_id;
|
||||
uint32_t pios_com_aux_id;
|
||||
uint32_t pios_com_spectrum_id;
|
||||
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
|
||||
|
||||
/**
|
||||
* 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();
|
||||
UAVObjectsInitializeAll();
|
||||
|
||||
AccelSensorInitialize();
|
||||
BaroSensorInitialize();
|
||||
MagSensorInitialize();
|
||||
GPSPositionSensorInitialize();
|
||||
GyroSensorInitialize();
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
/* Initialize the task monitor */
|
||||
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
|
||||
{
|
||||
uint32_t pios_tcp_telem_rf_id;
|
||||
if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
|
||||
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(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_tcp_com_driver, pios_tcp_telem_rf_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 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0
|
||||
{
|
||||
uint32_t pios_udp_telem_rf_id;
|
||||
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
|
||||
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(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_udp_com_driver, pios_udp_telem_rf_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 */
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_GPS)
|
||||
{
|
||||
uint32_t pios_tcp_gps_id;
|
||||
if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id,
|
||||
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
|
||||
NULL, 0)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
#endif /* if defined(PIOS_INCLUDE_COM) */
|
||||
|
||||
#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 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
85
flight/targets/boards/sparky2/firmware/sparky2.cpp
Normal file
85
flight/targets/boards/sparky2/firmware/sparky2.cpp
Normal file
@ -0,0 +1,85 @@
|
||||
/**
|
||||
****************************************************************************************
|
||||
* @addtogroup LibrePilotSystem LibrePilot System
|
||||
* @brief These files are the core system files for Sparky2.
|
||||
* They are the ground layer just above PiOS. In practice, Sparky2 actually starts
|
||||
* in the main() function of sparky2.cpp
|
||||
* @{
|
||||
* @addtogroup LibrePilotCore LibrePilot Core
|
||||
* @brief This is where the LP firmware starts. Those files also define the compile-time
|
||||
* options of the firmware.
|
||||
* @{
|
||||
* @file sparky2.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015
|
||||
* @brief Sets up and runs main 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
|
||||
*/
|
||||
|
||||
extern "C" {
|
||||
#include "inc/openpilot.h"
|
||||
#include <uavobjectsinit.h>
|
||||
#include <systemmod.h>
|
||||
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
}
|
||||
|
||||
/**
|
||||
* OpenPilot Main function:
|
||||
*
|
||||
* Initialize PiOS<BR>
|
||||
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
|
||||
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
|
||||
* 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() */
|
||||
vPortInitialiseBlocks();
|
||||
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
SystemModStart();
|
||||
|
||||
/* Start the FreeRTOS scheduler */
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
/* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT); \
|
||||
for (;;) { \
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \
|
||||
PIOS_DELAY_WaitmS(100); \
|
||||
}
|
||||
;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
339
flight/targets/boards/sparky2/pios_board.h
Normal file
339
flight/targets/boards/sparky2/pios_board.h
Normal file
@ -0,0 +1,339 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_board.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @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
|
||||
|
||||
#include <stdbool.h>
|
||||
// ------------------------
|
||||
// Timers and Channels Used
|
||||
// ------------------------
|
||||
/*
|
||||
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
|
||||
------+-----------+-----------+-----------+----------
|
||||
TIM1 | | | |
|
||||
TIM2 | --------------- PIOS_DELAY -----------------
|
||||
TIM3 | | | |
|
||||
TIM4 | | | |
|
||||
TIM5 | | | |
|
||||
TIM6 | | | |
|
||||
TIM7 | | | |
|
||||
TIM8 | | | |
|
||||
------+-----------+-----------+-----------+----------
|
||||
*/
|
||||
|
||||
// ------------------------
|
||||
// DMA Channels Used
|
||||
// ------------------------
|
||||
/* Channel 1 - */
|
||||
/* Channel 2 - SPI1 RX */
|
||||
/* Channel 3 - SPI1 TX */
|
||||
/* Channel 4 - SPI2 RX */
|
||||
/* Channel 5 - SPI2 TX */
|
||||
/* 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
|
||||
|
||||
// ------------------------
|
||||
// PIOS_LED
|
||||
// ------------------------
|
||||
#define PIOS_LED_HEARTBEAT 0
|
||||
#define PIOS_LED_ALARM 1
|
||||
#define PIOS_LED_LINK 2
|
||||
|
||||
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
|
||||
#define PIOS_LED_D1 2
|
||||
#define PIOS_LED_D2 3
|
||||
#define PIOS_LED_D3 4
|
||||
#define PIOS_LED_D4 5
|
||||
|
||||
#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1)
|
||||
#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1)
|
||||
#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1)
|
||||
|
||||
#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2)
|
||||
#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2)
|
||||
#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2)
|
||||
|
||||
#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3)
|
||||
#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3)
|
||||
#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3)
|
||||
|
||||
#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4)
|
||||
#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4)
|
||||
#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4)
|
||||
#endif /* PIOS_RFM22B_DEBUG_ON_TELEM */
|
||||
|
||||
// ------------------------
|
||||
// PIOS_SPI
|
||||
// See also pios_board.c
|
||||
// ------------------------
|
||||
#define PIOS_SPI_MAX_DEVS 3
|
||||
|
||||
// ------------------------
|
||||
// PIOS_WDG
|
||||
// ------------------------
|
||||
#define PIOS_WATCHDOG_TIMEOUT 250
|
||||
#define PIOS_WDG_REGISTER RTC_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_SENSORS 0x0010
|
||||
|
||||
// ------------------------
|
||||
// PIOS_I2C
|
||||
// See also pios_board.c
|
||||
// ------------------------
|
||||
#define PIOS_I2C_MAX_DEVS 3
|
||||
extern uint32_t pios_i2c_mag_pressure_adapter_id;
|
||||
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id)
|
||||
extern uint32_t pios_i2c_flexiport_adapter_id;
|
||||
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
|
||||
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
|
||||
#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
|
||||
|
||||
// -------------------------
|
||||
// PIOS_USART
|
||||
//
|
||||
// See also pios_board.c
|
||||
// -------------------------
|
||||
#define PIOS_USART_MAX_DEVS 5
|
||||
|
||||
// -------------------------
|
||||
// PIOS_COM
|
||||
//
|
||||
// See also pios_board.c
|
||||
// -------------------------
|
||||
#define PIOS_COM_MAX_DEVS 4
|
||||
extern uint32_t pios_com_telem_rf_id;
|
||||
extern uint32_t pios_com_rf_id;
|
||||
extern uint32_t pios_com_gps_id;
|
||||
extern uint32_t pios_com_telem_usb_id;
|
||||
extern uint32_t pios_com_bridge_id;
|
||||
extern uint32_t pios_com_vcp_id;
|
||||
extern uint32_t pios_com_hkosd_id;
|
||||
#define PIOS_COM_GPS (pios_com_gps_id)
|
||||
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
|
||||
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
|
||||
#define PIOS_COM_RF (pios_com_rf_id)
|
||||
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
|
||||
#define PIOS_COM_VCP (pios_com_vcp_id)
|
||||
#define PIOS_COM_OSDHK (pios_com_hkosd_id)
|
||||
|
||||
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
|
||||
extern uint32_t pios_com_debug_id;
|
||||
#define PIOS_COM_DEBUG (pios_com_debug_id)
|
||||
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
extern uint32_t pios_rfm22b_id;
|
||||
extern uint32_t pios_spi_telem_flash_id;
|
||||
#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id)
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
// -------------------------
|
||||
// Packet Handler
|
||||
// -------------------------
|
||||
#define RS_ECC_NPARITY 4
|
||||
#define PIOS_PH_MAX_PACKET 255
|
||||
#define PIOS_PH_WIN_SIZE 3
|
||||
#define PIOS_PH_MAX_CONNECTIONS 1
|
||||
extern uint32_t pios_packet_handler;
|
||||
#define PIOS_PACKET_HANDLER (pios_packet_handler)
|
||||
|
||||
// ------------------------
|
||||
// TELEMETRY
|
||||
// ------------------------
|
||||
#define TELEM_QUEUE_SIZE 80
|
||||
#define PIOS_TELEM_STACK_SIZE 800
|
||||
|
||||
// -------------------------
|
||||
// System Settings
|
||||
//
|
||||
// See also System_stm32f4xx.c
|
||||
// -------------------------
|
||||
// These macros are deprecated
|
||||
// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below
|
||||
// #define PIOS_MASTER_CLOCK
|
||||
// #define PIOS_PERIPHERAL_CLOCK
|
||||
// #define PIOS_PERIPHERAL_CLOCK
|
||||
|
||||
#define PIOS_SYSCLK 168000000
|
||||
// Peripherals that belongs to APB1 are:
|
||||
// DAC |PWR |CAN1,2
|
||||
// I2C1,2,3 |UART4,5 |USART3,2
|
||||
// I2S3Ext |SPI3/I2S3 |SPI2/I2S2
|
||||
// I2S2Ext |IWDG |WWDG
|
||||
// RTC/BKP reg
|
||||
// TIM2,3,4,5,6,7,12,13,14
|
||||
|
||||
// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2)
|
||||
// Default APB1 Prescaler = 4
|
||||
#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2)
|
||||
|
||||
// Peripherals belonging to APB2
|
||||
// SDIO |EXTI |SYSCFG |SPI1
|
||||
// ADC1,2,3
|
||||
// USART1,6
|
||||
// TIM1,8,9,10,11
|
||||
//
|
||||
// Default APB2 Prescaler = 2
|
||||
//
|
||||
#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK
|
||||
|
||||
// -------------------------
|
||||
// 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_RCVR
|
||||
// See also pios_board.c
|
||||
// ------------------------
|
||||
#define PIOS_RCVR_MAX_DEVS 3
|
||||
#define PIOS_RCVR_MAX_CHANNELS 12
|
||||
#define PIOS_GCSRCVR_TIMEOUT_MS 100
|
||||
#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200
|
||||
#define PIOS_OPLINK_RCVR_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 8
|
||||
|
||||
// -------------------------
|
||||
// Receiver SPEKTRUM input
|
||||
// -------------------------
|
||||
#define PIOS_SPEKTRUM_MAX_DEVS 2
|
||||
#define PIOS_SPEKTRUM_NUM_INPUTS 12
|
||||
|
||||
// -------------------------
|
||||
// Receiver S.Bus input
|
||||
// -------------------------
|
||||
#define PIOS_SBUS_MAX_DEVS 1
|
||||
#define PIOS_SBUS_NUM_INPUTS (16 + 2)
|
||||
|
||||
// -------------------------
|
||||
// Receiver HOTT input
|
||||
// -------------------------
|
||||
#define PIOS_HOTT_MAX_DEVS 1
|
||||
#define PIOS_HOTT_NUM_INPUTS 32
|
||||
|
||||
// -------------------------
|
||||
// Receiver EX.Bus input
|
||||
// -------------------------
|
||||
#define PIOS_EXBUS_MAX_DEVS 1
|
||||
#define PIOS_EXBUS_NUM_INPUTS 16
|
||||
|
||||
// -------------------------
|
||||
// Receiver Multiplex SRXL input
|
||||
// -------------------------
|
||||
#define PIOS_SRXL_MAX_DEVS 1
|
||||
#define PIOS_SRXL_NUM_INPUTS 16
|
||||
|
||||
// -------------------------
|
||||
// Receiver DSM input
|
||||
// -------------------------
|
||||
#define PIOS_DSM_MAX_DEVS 2
|
||||
#define PIOS_DSM_NUM_INPUTS 12
|
||||
|
||||
// -------------------------
|
||||
// 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 6
|
||||
|
||||
// -------------------------
|
||||
// ADC
|
||||
// PIOS_ADC_PinGet(0) = Current sensor
|
||||
// PIOS_ADC_PinGet(1) = Voltage sensor
|
||||
// PIOS_ADC_PinGet(4) = VREF
|
||||
// PIOS_ADC_PinGet(5) = Temperature sensor
|
||||
// -------------------------
|
||||
#define PIOS_DMA_PIN_CONFIG \
|
||||
{ \
|
||||
{ GPIOC, GPIO_Pin_3, ADC_Channel_13, false }, /* batt/sonar pin 3 */ \
|
||||
{ GPIOC, GPIO_Pin_2, ADC_Channel_12, false }, /* batt/sonar pin 4 */ \
|
||||
{ GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \
|
||||
{ GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \
|
||||
{ GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \
|
||||
{ GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \
|
||||
{ NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \
|
||||
{ NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \
|
||||
}
|
||||
|
||||
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
|
||||
/* which is annoying because this then determines the rate at which we generate buffer turnover events */
|
||||
/* the objective here is to get enough buffer space to support 100Hz averaging rate */
|
||||
#define PIOS_ADC_NUM_CHANNELS 8
|
||||
#define PIOS_ADC_MAX_OVERSAMPLING 2
|
||||
#define PIOS_ADC_USE_ADC2 0
|
||||
|
||||
#define PIOS_ADC_USE_TEMP_SENSOR
|
||||
#define PIOS_ADC_TEMPERATURE_PIN 7
|
||||
|
||||
// -------------------------
|
||||
// USB
|
||||
// -------------------------
|
||||
#define PIOS_USB_MAX_DEVS 1
|
||||
#define PIOS_USB_ENABLED 1 /* Should remove all references to this */
|
||||
#define PIOS_USB_HID_MAX_DEVS 1
|
||||
|
||||
#endif /* PIOS_BOARD_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
99
flight/targets/boards/sparky2/pios_usb_board_data.c
Normal file
99
flight/targets/boards/sparky2/pios_usb_board_data.c
Normal file
@ -0,0 +1,99 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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.h> /* PIOS_SYS_SerialNumberGet */
|
||||
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
|
||||
#include <pios_usb_util.h> /* PIOS_USB_UTIL_AsciiToUtf8 */
|
||||
|
||||
static const uint8_t usb_product_id[22] = {
|
||||
sizeof(usb_product_id),
|
||||
USB_DESC_TYPE_STRING,
|
||||
'S', 0,
|
||||
'p', 0,
|
||||
'a', 0,
|
||||
'r', 0,
|
||||
'k', 0,
|
||||
'y', 0,
|
||||
' ', 0,
|
||||
'2', 0,
|
||||
'.', 0,
|
||||
'0', 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;
|
||||
}
|
@ -78,9 +78,16 @@ int main()
|
||||
struct pios_board_info *new_board_info_blob = (struct pios_board_info *)((uint32_t)embedded_image_start + board_info_blob_offset);
|
||||
|
||||
/* Compare the two board info blobs to make sure they're for the same HW revision */
|
||||
if ((pios_board_info_blob.magic != new_board_info_blob->magic) ||
|
||||
(pios_board_info_blob.board_type != new_board_info_blob->board_type) ||
|
||||
(pios_board_info_blob.board_rev != new_board_info_blob->board_rev)) {
|
||||
if ((pios_board_info_blob.magic != new_board_info_blob->magic)
|
||||
#if PIOS_USB_BOARD_PRODUCT_ID == USB_PRODUCT_ID_SPARKY2
|
||||
|| (((pios_board_info_blob.board_type != new_board_info_blob->board_type)
|
||||
|| (pios_board_info_blob.board_rev != new_board_info_blob->board_rev))
|
||||
&& ((pios_board_info_blob.board_type != 0x0b) || (pios_board_info_blob.board_rev != 0x01)))
|
||||
#else
|
||||
|| (pios_board_info_blob.board_type != new_board_info_blob->board_type)
|
||||
|| (pios_board_info_blob.board_rev != new_board_info_blob->board_rev)
|
||||
#endif
|
||||
) {
|
||||
error(PIOS_LED_HEARTBEAT, 2);
|
||||
}
|
||||
|
||||
|
@ -15,8 +15,8 @@ int main(int argc, char *argv[])
|
||||
bool use_serial = false;
|
||||
bool verify;
|
||||
bool debug = false;
|
||||
bool umodereset = false;
|
||||
OP_DFU::Actions action;
|
||||
// bool umodereset = false;
|
||||
OP_DFU::Actions action = OP_DFU::actionNone;
|
||||
QString file;
|
||||
QString serialport;
|
||||
QString description;
|
||||
@ -27,7 +27,7 @@ int main(int argc, char *argv[])
|
||||
debug = true;
|
||||
}
|
||||
if (args.contains("-ur")) {
|
||||
umodereset = true;
|
||||
// umodereset = true;
|
||||
}
|
||||
standardOutput << "OpenPilot serial firmware uploader tool." << endl;
|
||||
if (args.indexOf(PROGRAMFW) + 1 < args.length()) {
|
||||
@ -57,17 +57,17 @@ int main(int argc, char *argv[])
|
||||
}
|
||||
action = OP_DFU::actionProgram;
|
||||
} else if (args.contains(COMPARECRC) || args.contains(COMPAREALL)) {
|
||||
int index;
|
||||
// int index;
|
||||
if (args.contains(COMPARECRC)) {
|
||||
index = args.indexOf(COMPARECRC);
|
||||
// index = args.indexOf(COMPARECRC);
|
||||
action = OP_DFU::actionCompareCrc;
|
||||
} else {
|
||||
index = args.indexOf(COMPAREALL);
|
||||
// index = args.indexOf(COMPAREALL);
|
||||
action = OP_DFU::actionCompareAll;
|
||||
}
|
||||
} else if (args.contains(DOWNLOAD)) {
|
||||
int index;
|
||||
index = args.indexOf(DOWNLOAD);
|
||||
// int index;
|
||||
// index = args.indexOf(DOWNLOAD);
|
||||
action = OP_DFU::actionDownload;
|
||||
} else if (args.contains(STATUSREQUEST)) {
|
||||
action = OP_DFU::actionStatusReq;
|
||||
|
@ -1119,11 +1119,12 @@ 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
|
||||
#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
|
||||
#define BOARD_ID_SPARKY2 0x92
|
||||
|
||||
/**
|
||||
Gets the type of board connected
|
||||
@ -1152,6 +1153,9 @@ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum)
|
||||
case BOARD_ID_REVO: // Revo board
|
||||
brdType = eBoardRevo;
|
||||
break;
|
||||
case BOARD_ID_SPARKY2: // Sparky2 board
|
||||
brdType = eBoardSparky2;
|
||||
break;
|
||||
}
|
||||
return brdType;
|
||||
}
|
||||
|
@ -75,6 +75,7 @@ enum Status {
|
||||
};
|
||||
|
||||
enum Actions {
|
||||
actionNone,
|
||||
actionProgram,
|
||||
actionProgramAndVerify,
|
||||
actionDownload,
|
||||
@ -109,6 +110,7 @@ enum eBoardType {
|
||||
eBoardPip = 3,
|
||||
eBoardCC = 4,
|
||||
eBoardRevo = 9,
|
||||
eBoardSparky2 = 0x92,
|
||||
};
|
||||
|
||||
struct device {
|
||||
|
@ -620,8 +620,9 @@ QSGNode *OSGViewport::updatePaintNode(QSGNode *node, QQuickItem::UpdatePaintNode
|
||||
if (!node) {
|
||||
node = QQuickFramebufferObject::updatePaintNode(node, nodeData);
|
||||
QSGSimpleTextureNode *n = static_cast<QSGSimpleTextureNode *>(node);
|
||||
if (n)
|
||||
if (n) {
|
||||
n->setTextureCoordinatesTransform(QSGSimpleTextureNode::MirrorVertically);
|
||||
}
|
||||
return node;
|
||||
}
|
||||
return QQuickFramebufferObject::updatePaintNode(node, nodeData);
|
||||
|
@ -55,6 +55,7 @@ HEADERS += \
|
||||
calibration/calibrationuiutils.h \
|
||||
configoplinkwidget.h \
|
||||
configrevonanohwwidget.h \
|
||||
configsparky2hwwidget.h \
|
||||
failsafechannelform.h
|
||||
|
||||
SOURCES += \
|
||||
@ -97,6 +98,7 @@ SOURCES += \
|
||||
calibration/gyrobiascalibrationmodel.cpp \
|
||||
configoplinkwidget.cpp \
|
||||
configrevonanohwwidget.cpp \
|
||||
configsparky2hwwidget.cpp \
|
||||
failsafechannelform.cpp
|
||||
|
||||
FORMS += \
|
||||
@ -123,6 +125,7 @@ FORMS += \
|
||||
configrevohwwidget.ui \
|
||||
oplink.ui \
|
||||
configrevonanohwwidget.ui \
|
||||
configsparky2hwwidget.ui \
|
||||
failsafechannelform.ui
|
||||
|
||||
OTHER_FILES += \
|
||||
|
@ -57,5 +57,6 @@
|
||||
<file>images/error.svg</file>
|
||||
<file>images/nano_top.png</file>
|
||||
<file>images/cc3d_top.png</file>
|
||||
<file>images/sparky2_top.png</file>
|
||||
</qresource>
|
||||
</RCC>
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file configgadgetwidget.cpp
|
||||
* @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup ConfigPlugin Config Plugin
|
||||
@ -39,6 +40,7 @@
|
||||
#include "configoplinkwidget.h"
|
||||
#include "configrevowidget.h"
|
||||
#include "configrevonanohwwidget.h"
|
||||
#include "configsparky2hwwidget.h"
|
||||
#include "defaultattitudewidget.h"
|
||||
#include "defaulthwsettingswidget.h"
|
||||
#include "uavobjectutilmanager.h"
|
||||
@ -200,6 +202,12 @@ void ConfigGadgetWidget::onAutopilotConnect()
|
||||
qwd = new ConfigRevoNanoHWWidget(this);
|
||||
}
|
||||
stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd);
|
||||
} else if ((board & 0xff00) == 0x9200) {
|
||||
// Sparky2
|
||||
QWidget *qwd = new ConfigRevoWidget(this);
|
||||
stackWidget->replaceTab(ConfigGadgetWidget::sensors, qwd);
|
||||
qwd = new ConfigSparky2HWWidget(this);
|
||||
stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd);
|
||||
} else {
|
||||
// Unknown board
|
||||
qDebug() << "Unknown board " << board;
|
||||
|
@ -1,5 +1,5 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
****************************************************************************************
|
||||
*
|
||||
* @file configoplinkwidget.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
@ -9,7 +9,7 @@
|
||||
* @addtogroup ConfigPlugin Config Plugin
|
||||
* @{
|
||||
* @brief The Configuration Gadget used to configure the OPLink and Revo modem
|
||||
*****************************************************************************/
|
||||
***************************************************************************************/
|
||||
/*
|
||||
* 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
|
||||
@ -207,7 +207,8 @@ void ConfigOPLinkWidget::updateSettings(UAVObject *object)
|
||||
// Enable components based on the board type connected.
|
||||
UAVObjectField *board_type_field = oplinkStatusObj->getField("BoardType");
|
||||
switch (board_type_field->getValue().toInt()) {
|
||||
case 0x09: // Revolution
|
||||
case 0x09: // Revolution, DiscoveryF4Bare, RevoNano, RevoProto
|
||||
case 0x92: // Sparky2
|
||||
m_oplink->MainPort->setVisible(false);
|
||||
m_oplink->MainPortLabel->setVisible(false);
|
||||
m_oplink->FlexiPort->setVisible(false);
|
||||
@ -231,7 +232,7 @@ void ConfigOPLinkWidget::updateSettings(UAVObject *object)
|
||||
connect(m_oplink->MainPort, SIGNAL(currentIndexChanged(int)), this, SLOT(updatePPMOptions()));
|
||||
connect(m_oplink->FlexiPort, SIGNAL(currentIndexChanged(int)), this, SLOT(updatePPMOptions()));
|
||||
break;
|
||||
case 0x0A: // OPLink?
|
||||
case 0x0a: // OPLink? (No. This is wrong. 0x0A is gpsplatinum.)
|
||||
m_oplink->MainPort->setVisible(true);
|
||||
m_oplink->MainPortLabel->setVisible(true);
|
||||
m_oplink->FlexiPort->setVisible(true);
|
||||
|
@ -381,6 +381,10 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj)
|
||||
// Revolution Nano
|
||||
bankLabels << "1 (1)" << "2 (2,7,11)" << "3 (3)" << "4 (4)" << "5 (5-6)" << "6 (8-10,12)";
|
||||
channelBanks << 1 << 2 << 3 << 4 << 5 << 5 << 2 << 6 << 6 << 6 << 2 << 6;
|
||||
} else if (board == 0x9201) {
|
||||
// Sparky2
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
288
ground/gcs/src/plugins/config/configsparky2hwwidget.cpp
Normal file
288
ground/gcs/src/plugins/config/configsparky2hwwidget.cpp
Normal file
@ -0,0 +1,288 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file configsparky2hwwidget.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup ConfigPlugin Config Plugin
|
||||
* @{
|
||||
* @brief Sparky2 hardware 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
|
||||
*/
|
||||
#include "configsparky2hwwidget.h"
|
||||
|
||||
#include "ui_configsparky2hwwidget.h"
|
||||
|
||||
#include <extensionsystem/pluginmanager.h>
|
||||
#include <coreplugin/generalsettings.h>
|
||||
|
||||
#include "hwsettings.h"
|
||||
|
||||
#include <QDebug>
|
||||
#include <QDesktopServices>
|
||||
#include <QUrl>
|
||||
#include <QMessageBox>
|
||||
|
||||
ConfigSparky2HWWidget::ConfigSparky2HWWidget(QWidget *parent) : ConfigTaskWidget(parent), m_refreshing(true)
|
||||
{
|
||||
m_ui = new Ui_Sparky2HWWidget();
|
||||
m_ui->setupUi(this);
|
||||
|
||||
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||||
Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
|
||||
if (!settings->useExpertMode()) {
|
||||
m_ui->saveTelemetryToRAM->setEnabled(false);
|
||||
m_ui->saveTelemetryToRAM->setVisible(false);
|
||||
}
|
||||
|
||||
addApplySaveButtons(m_ui->saveTelemetryToRAM, m_ui->saveTelemetryToSD);
|
||||
|
||||
addWidgetBinding("HwSettings", "SPK2_FlexiPort", m_ui->cbFlexi);
|
||||
addWidgetBinding("HwSettings", "SPK2_MainPort", m_ui->cbMain);
|
||||
addWidgetBinding("HwSettings", "SPK2_RcvrPort", m_ui->cbRcvr);
|
||||
addWidgetBinding("HwSettings", "SPK2_I2CPort", m_ui->cbI2C);
|
||||
|
||||
addWidgetBinding("HwSettings", "USB_HIDPort", m_ui->cbUSBHIDFunction);
|
||||
addWidgetBinding("HwSettings", "USB_VCPPort", m_ui->cbUSBVCPFunction);
|
||||
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbUSBVCPSpeed);
|
||||
|
||||
addWidgetBinding("HwSettings", "TelemetrySpeed", m_ui->cbFlexiTelemSpeed);
|
||||
addWidgetBinding("HwSettings", "GPSSpeed", m_ui->cbFlexiGPSSpeed);
|
||||
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbFlexiComSpeed);
|
||||
|
||||
addWidgetBinding("HwSettings", "TelemetrySpeed", m_ui->cbMainTelemSpeed);
|
||||
addWidgetBinding("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed);
|
||||
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed);
|
||||
|
||||
// Add Gps protocol configuration
|
||||
addWidgetBinding("GPSSettings", "DataProtocol", m_ui->cbMainGPSProtocol);
|
||||
addWidgetBinding("GPSSettings", "DataProtocol", m_ui->cbFlexiGPSProtocol);
|
||||
|
||||
connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
|
||||
|
||||
setupCustomCombos();
|
||||
enableControls(true);
|
||||
populateWidgets();
|
||||
refreshWidgetsValues();
|
||||
forceConnectedState();
|
||||
m_refreshing = false;
|
||||
}
|
||||
|
||||
ConfigSparky2HWWidget::~ConfigSparky2HWWidget()
|
||||
{
|
||||
// Do nothing
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::setupCustomCombos()
|
||||
{
|
||||
connect(m_ui->cbUSBHIDFunction, SIGNAL(currentIndexChanged(int)), this, SLOT(usbHIDPortChanged(int)));
|
||||
connect(m_ui->cbUSBVCPFunction, SIGNAL(currentIndexChanged(int)), this, SLOT(usbVCPPortChanged(int)));
|
||||
|
||||
// m_ui->cbSonar->addItem(tr("Disabled"));
|
||||
// m_ui->cbSonar->setCurrentIndex(0);
|
||||
// m_ui->cbSonar->setEnabled(false);
|
||||
|
||||
connect(m_ui->cbFlexi, SIGNAL(currentIndexChanged(int)), this, SLOT(flexiPortChanged(int)));
|
||||
connect(m_ui->cbMain, SIGNAL(currentIndexChanged(int)), this, SLOT(mainPortChanged(int)));
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::refreshWidgetsValues(UAVObject *obj)
|
||||
{
|
||||
m_refreshing = true;
|
||||
ConfigTaskWidget::refreshWidgetsValues(obj);
|
||||
|
||||
usbVCPPortChanged(0);
|
||||
mainPortChanged(0);
|
||||
flexiPortChanged(0);
|
||||
m_refreshing = false;
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::updateObjectsFromWidgets()
|
||||
{
|
||||
ConfigTaskWidget::updateObjectsFromWidgets();
|
||||
|
||||
HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
|
||||
HwSettings::DataFields data = hwSettings->getData();
|
||||
|
||||
// If any port is configured to be GPS port, enable GPS module if it is not enabled.
|
||||
// Otherwise disable GPS module.
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_GPS)
|
||||
|| isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_GPS)) {
|
||||
data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_ENABLED;
|
||||
} else {
|
||||
data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_DISABLED;
|
||||
}
|
||||
|
||||
hwSettings->setData(data);
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::usbVCPPortChanged(int index)
|
||||
{
|
||||
Q_UNUSED(index);
|
||||
|
||||
bool vcpComBridgeEnabled = isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_COMBRIDGE);
|
||||
|
||||
m_ui->lblUSBVCPSpeed->setVisible(vcpComBridgeEnabled);
|
||||
m_ui->cbUSBVCPSpeed->setVisible(vcpComBridgeEnabled);
|
||||
|
||||
if (!vcpComBridgeEnabled && isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_COMBRIDGE)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
enableComboBoxOptionItem(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_COMBRIDGE, vcpComBridgeEnabled);
|
||||
|
||||
if (!vcpComBridgeEnabled && isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_COMBRIDGE)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
enableComboBoxOptionItem(m_ui->cbMain, HwSettings::SPK2_MAINPORT_COMBRIDGE, vcpComBridgeEnabled);
|
||||
|
||||
// _DEBUGCONSOLE modes are mutual exclusive
|
||||
if (isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DEBUGCONSOLE)) {
|
||||
if (isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
}
|
||||
|
||||
// _USBTELEMETRY modes are mutual exclusive
|
||||
if (isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_USBTELEMETRY)
|
||||
&& isComboboxOptionSelected(m_ui->cbUSBHIDFunction, HwSettings::USB_HIDPORT_USBTELEMETRY)) {
|
||||
setComboboxSelectedOption(m_ui->cbUSBHIDFunction, HwSettings::USB_HIDPORT_DISABLED);
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::usbHIDPortChanged(int index)
|
||||
{
|
||||
Q_UNUSED(index);
|
||||
|
||||
// _USBTELEMETRY modes are mutual exclusive
|
||||
if (isComboboxOptionSelected(m_ui->cbUSBHIDFunction, HwSettings::USB_HIDPORT_USBTELEMETRY)
|
||||
&& isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_USBTELEMETRY)) {
|
||||
setComboboxSelectedOption(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DISABLED);
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::flexiPortChanged(int index)
|
||||
{
|
||||
Q_UNUSED(index);
|
||||
|
||||
m_ui->cbFlexiTelemSpeed->setVisible(false);
|
||||
m_ui->cbFlexiGPSSpeed->setVisible(false);
|
||||
m_ui->cbFlexiComSpeed->setVisible(false);
|
||||
m_ui->lblFlexiSpeed->setVisible(true);
|
||||
|
||||
// Add Gps protocol configuration
|
||||
m_ui->cbFlexiGPSProtocol->setVisible(false);
|
||||
m_ui->lbFlexiGPSProtocol->setVisible(false);
|
||||
|
||||
switch (getComboboxSelectedOption(m_ui->cbFlexi)) {
|
||||
case HwSettings::SPK2_FLEXIPORT_TELEMETRY:
|
||||
m_ui->cbFlexiTelemSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_TELEMETRY)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_FLEXIPORT_GPS:
|
||||
// Add Gps protocol configuration
|
||||
m_ui->cbFlexiGPSProtocol->setVisible(true);
|
||||
m_ui->lbFlexiGPSProtocol->setVisible(true);
|
||||
|
||||
m_ui->cbFlexiGPSSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_GPS)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_FLEXIPORT_COMBRIDGE:
|
||||
m_ui->cbFlexiComSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_COMBRIDGE)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_FLEXIPORT_DEBUGCONSOLE:
|
||||
m_ui->cbFlexiComSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbMain, HwSettings::SPK2_MAINPORT_DISABLED);
|
||||
}
|
||||
if (isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
m_ui->lblFlexiSpeed->setVisible(false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::mainPortChanged(int index)
|
||||
{
|
||||
Q_UNUSED(index);
|
||||
|
||||
m_ui->cbMainTelemSpeed->setVisible(false);
|
||||
m_ui->cbMainGPSSpeed->setVisible(false);
|
||||
m_ui->cbMainComSpeed->setVisible(false);
|
||||
m_ui->lblMainSpeed->setVisible(true);
|
||||
|
||||
// Add Gps protocol configuration
|
||||
m_ui->cbMainGPSProtocol->setVisible(false);
|
||||
m_ui->lbMainGPSProtocol->setVisible(false);
|
||||
|
||||
switch (getComboboxSelectedOption(m_ui->cbMain)) {
|
||||
case HwSettings::SPK2_MAINPORT_TELEMETRY:
|
||||
m_ui->cbMainTelemSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_TELEMETRY)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_MAINPORT_GPS:
|
||||
// Add Gps protocol configuration
|
||||
m_ui->cbMainGPSProtocol->setVisible(true);
|
||||
m_ui->lbMainGPSProtocol->setVisible(true);
|
||||
|
||||
m_ui->cbMainGPSSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_GPS)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_MAINPORT_COMBRIDGE:
|
||||
m_ui->cbMainComSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_COMBRIDGE)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
case HwSettings::SPK2_MAINPORT_DEBUGCONSOLE:
|
||||
m_ui->cbMainComSpeed->setVisible(true);
|
||||
if (isComboboxOptionSelected(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbFlexi, HwSettings::SPK2_FLEXIPORT_DISABLED);
|
||||
}
|
||||
if (isComboboxOptionSelected(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DEBUGCONSOLE)) {
|
||||
setComboboxSelectedOption(m_ui->cbUSBVCPFunction, HwSettings::USB_VCPPORT_DISABLED);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
m_ui->lblMainSpeed->setVisible(false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigSparky2HWWidget::openHelp()
|
||||
{
|
||||
QDesktopServices::openUrl(QUrl(QString(WIKI_URL_ROOT) + QString("Sparky2+Configuration"),
|
||||
QUrl::StrictMode));
|
||||
}
|
63
ground/gcs/src/plugins/config/configsparky2hwwidget.h
Normal file
63
ground/gcs/src/plugins/config/configsparky2hwwidget.h
Normal file
@ -0,0 +1,63 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file configsparky2hwwidget.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup ConfigPlugin Config Plugin
|
||||
* @{
|
||||
* @brief Sparky2 hardware 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 CONFIGSPARKY2HWWIDGET_H
|
||||
#define CONFIGSPARKY2HWWIDGET_H
|
||||
|
||||
#include "../uavobjectwidgetutils/configtaskwidget.h"
|
||||
|
||||
class Ui_Sparky2HWWidget;
|
||||
|
||||
class UAVObject;
|
||||
|
||||
class QWidget;
|
||||
|
||||
class ConfigSparky2HWWidget : public ConfigTaskWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
ConfigSparky2HWWidget(QWidget *parent = 0);
|
||||
~ConfigSparky2HWWidget();
|
||||
|
||||
private:
|
||||
Ui_Sparky2HWWidget *m_ui;
|
||||
bool m_refreshing;
|
||||
void setupCustomCombos();
|
||||
|
||||
protected slots:
|
||||
void refreshWidgetsValues(UAVObject *obj = NULL);
|
||||
void updateObjectsFromWidgets();
|
||||
|
||||
private slots:
|
||||
void usbVCPPortChanged(int index);
|
||||
void usbHIDPortChanged(int index);
|
||||
void flexiPortChanged(int index);
|
||||
void mainPortChanged(int index);
|
||||
void openHelp();
|
||||
};
|
||||
|
||||
#endif // CONFIGSPARKY2HWWIDGET_H
|
771
ground/gcs/src/plugins/config/configsparky2hwwidget.ui
Normal file
771
ground/gcs/src/plugins/config/configsparky2hwwidget.ui
Normal file
@ -0,0 +1,771 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>Sparky2HWWidget</class>
|
||||
<widget class="QWidget" name="Sparky2HWWidget">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>834</width>
|
||||
<height>742</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>Form</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<item>
|
||||
<widget class="QTabWidget" name="tabWidget">
|
||||
<property name="currentIndex">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="tab">
|
||||
<property name="autoFillBackground">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<attribute name="title">
|
||||
<string>HW settings</string>
|
||||
</attribute>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_3">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QScrollArea" name="scrollArea">
|
||||
<property name="palette">
|
||||
<palette>
|
||||
<active>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>255</red>
|
||||
<green>255</green>
|
||||
<blue>255</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</active>
|
||||
<inactive>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>255</red>
|
||||
<green>255</green>
|
||||
<blue>255</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</inactive>
|
||||
<disabled>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</disabled>
|
||||
</palette>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">border-color: rgb(255, 0, 0);</string>
|
||||
</property>
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::NoFrame</enum>
|
||||
</property>
|
||||
<property name="frameShadow">
|
||||
<enum>QFrame::Plain</enum>
|
||||
</property>
|
||||
<property name="widgetResizable">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QWidget" name="scrollAreaWidgetContents">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-258</y>
|
||||
<width>794</width>
|
||||
<height>927</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_3">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<item row="3" column="2">
|
||||
<spacer name="verticalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<spacer name="horizontalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="2" column="3">
|
||||
<spacer name="horizontalSpacer_5">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_6">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>16777215</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Changes on this page only take effect after board reset or power cycle</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<spacer name="verticalSpacer_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="2" column="2">
|
||||
<layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0">
|
||||
<item row="10" column="0">
|
||||
<widget class="QComboBox" name="cbMainGPSProtocol"/>
|
||||
</item>
|
||||
<item row="7" column="0">
|
||||
<widget class="QComboBox" name="cbMain"/>
|
||||
</item>
|
||||
<item row="20" column="2">
|
||||
<spacer name="horizontalSpacer_10">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>80</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="18" column="0">
|
||||
<spacer name="horizontalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Minimum</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>120</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="19" column="3">
|
||||
<widget class="QComboBox" name="cbI2C"/>
|
||||
</item>
|
||||
<item row="20" column="3">
|
||||
<spacer name="horizontalSpacer_9">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Minimum</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>120</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="20" column="4">
|
||||
<spacer name="horizontalSpacer_8">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Minimum</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>70</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="18" column="3" alignment="Qt::AlignHCenter">
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="text">
|
||||
<string>I2C (under)</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="20" column="1">
|
||||
<spacer name="horizontalSpacer_7">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Minimum</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>120</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="14" column="0">
|
||||
<widget class="QComboBox" name="cbUSBVCPFunction"/>
|
||||
</item>
|
||||
<item row="18" column="4">
|
||||
<spacer name="horizontalSpacer_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>50</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="8" column="0">
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_4">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_4">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QLabel" name="lblMainSpeed">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Speed</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbMainTelemSpeed"/>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbMainGPSSpeed"/>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbMainComSpeed"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="16" column="0">
|
||||
<widget class="QComboBox" name="cbUSBVCPSpeed">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="12" column="0">
|
||||
<widget class="QComboBox" name="cbUSBHIDFunction"/>
|
||||
</item>
|
||||
<item row="15" column="0" alignment="Qt::AlignHCenter">
|
||||
<widget class="QLabel" name="lblUSBVCPSpeed">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Speed</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="13" column="0" alignment="Qt::AlignHCenter">
|
||||
<widget class="QLabel" name="label_8">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>USB VCP Function</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="11" column="0" alignment="Qt::AlignHCenter">
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>USB HID Function</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="1" rowspan="12" colspan="4">
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>350</width>
|
||||
<height>350</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="pixmap">
|
||||
<pixmap resource="configgadget.qrc">:/configgadget/images/sparky2_top.png</pixmap>
|
||||
</property>
|
||||
<property name="scaledContents">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::NoTextInteraction</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="3">
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="text">
|
||||
<string>Receiver Port</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_9">
|
||||
<property name="text">
|
||||
<string>Flexi Port</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<widget class="QComboBox" name="cbRcvr"/>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_2">
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbFlexiTelemSpeed"/>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbFlexiGPSSpeed"/>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="cbFlexiComSpeed">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="lblFlexiSpeed">
|
||||
<property name="text">
|
||||
<string>Speed</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="lbFlexiGPSProtocol">
|
||||
<property name="text">
|
||||
<string>Protocol</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="9" column="0">
|
||||
<widget class="QLabel" name="lbMainGPSProtocol">
|
||||
<property name="text">
|
||||
<string>Protocol</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QComboBox" name="cbFlexi"/>
|
||||
</item>
|
||||
<item row="5" column="1">
|
||||
<widget class="QComboBox" name="cbFlexiGPSProtocol"/>
|
||||
</item>
|
||||
<item row="6" column="0">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="text">
|
||||
<string>Main Port</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignBottom|Qt::AlignHCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_2">
|
||||
<property name="spacing">
|
||||
<number>4</number>
|
||||
</property>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>369</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="cchwHelp">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>25</width>
|
||||
<height>25</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>25</width>
|
||||
<height>25</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Takes you to the wiki page</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="icon">
|
||||
<iconset resource="../coreplugin/core.qrc">
|
||||
<normaloff>:/core/images/helpicon.svg</normaloff>:/core/images/helpicon.svg</iconset>
|
||||
</property>
|
||||
<property name="iconSize">
|
||||
<size>
|
||||
<width>25</width>
|
||||
<height>25</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="flat">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="saveTelemetryToRAM">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>16777215</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="palette">
|
||||
<palette>
|
||||
<active>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>255</red>
|
||||
<green>255</green>
|
||||
<blue>255</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</active>
|
||||
<inactive>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="0">
|
||||
<red>255</red>
|
||||
<green>255</green>
|
||||
<blue>255</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</inactive>
|
||||
<disabled>
|
||||
<colorrole role="Base">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
<colorrole role="Window">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>232</red>
|
||||
<green>232</green>
|
||||
<blue>232</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</colorrole>
|
||||
</disabled>
|
||||
</palette>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Send to board but don't write in SD.
|
||||
Beware of not locking yourself out!</string>
|
||||
</property>
|
||||
<property name="autoFillBackground">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true"/>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Apply</string>
|
||||
</property>
|
||||
<property name="iconSize">
|
||||
<size>
|
||||
<width>16</width>
|
||||
<height>16</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="checkable">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="saveTelemetryToSD">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>16777215</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Applies and Saves all settings to SD.
|
||||
Beware of not locking yourself out!</string>
|
||||
</property>
|
||||
<property name="autoFillBackground">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true"/>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Save</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<resources>
|
||||
<include location="../coreplugin/core.qrc"/>
|
||||
<include location="configgadget.qrc"/>
|
||||
</resources>
|
||||
<connections/>
|
||||
</ui>
|
@ -656,8 +656,8 @@ void ConfigStabilizationWidget::onBoardConnected()
|
||||
|
||||
Q_ASSERT(utilMngr);
|
||||
boardModel = utilMngr->getBoardModel();
|
||||
// If Revolution board enable Althold tab, otherwise disable it
|
||||
ui->AltitudeHold->setEnabled((boardModel & 0xff00) == 0x0900);
|
||||
// If Revolution/Sparky2 board enable Althold tab, otherwise disable it
|
||||
ui->AltitudeHold->setEnabled(((boardModel & 0xff00) == 0x0900) || ((boardModel & 0xff00) == 0x9200));
|
||||
}
|
||||
|
||||
void ConfigStabilizationWidget::stabBankChanged(int index)
|
||||
@ -688,8 +688,8 @@ void ConfigStabilizationWidget::stabBankChanged(int index)
|
||||
|
||||
bool ConfigStabilizationWidget::shouldObjectBeSaved(UAVObject *object)
|
||||
{
|
||||
// AltitudeHoldSettings should only be saved for Revolution board to avoid error.
|
||||
if ((boardModel & 0xff00) != 0x0900) {
|
||||
// AltitudeHoldSettings should only be saved for Revolution/Sparky2 board to avoid error.
|
||||
if (((boardModel & 0xff00) != 0x0900) && ((boardModel & 0xff00) != 0x9200)) {
|
||||
return dynamic_cast<AltitudeHoldSettings *>(object) == 0;
|
||||
} else {
|
||||
return true;
|
||||
|
BIN
ground/gcs/src/plugins/config/images/sparky2_top.png
Normal file
BIN
ground/gcs/src/plugins/config/images/sparky2_top.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 110 KiB |
@ -603,7 +603,7 @@ void FlightLogManager::connectionStatusChanged()
|
||||
if (m_telemtryManager->isConnected()) {
|
||||
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||||
UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
|
||||
setBoardConnected(utilMngr->getBoardModel() == 0x0903);
|
||||
setBoardConnected(utilMngr->getBoardModel() == 0x0903 || utilMngr->getBoardModel() == 0x0904 || utilMngr->getBoardModel() == 0x0905 || utilMngr->getBoardModel() == 0x9201);
|
||||
} else {
|
||||
setBoardConnected(false);
|
||||
}
|
||||
|
@ -31,10 +31,10 @@
|
||||
#include <QDebug>
|
||||
|
||||
#ifdef OPHID_DEBUG_ON
|
||||
#define OPHID_DEBUG(fmt, args ...) qDebug("[DEBUG] "fmt,##args)
|
||||
#define OPHID_TRACE(fmt, args ...) qDebug("[TRACE] %s:%s:%d: "fmt, __FILE__, __func__, __LINE__,##args)
|
||||
#define OPHID_ERROR(fmt, args ...) qDebug("[ERROR] %s:%s:%d: "fmt, __FILE__, __func__, __LINE__,##args)
|
||||
#define OPHID_WARNING(fmt, args ...) qDebug("[WARNING] "fmt,##args)
|
||||
#define OPHID_DEBUG(fmt, args ...) qDebug("[DEBUG] "##fmt, args)
|
||||
#define OPHID_TRACE(fmt, args ...) qDebug("[TRACE] %s:%s:%d: "##fmt, __FILE__, __func__, __LINE__, args)
|
||||
#define OPHID_ERROR(fmt, args ...) qDebug("[ERROR] %s:%s:%d: "##fmt, __FILE__, __func__, __LINE__, args)
|
||||
#define OPHID_WARNING(fmt, args ...) qDebug("[WARNING] "##fmt, args)
|
||||
#else
|
||||
#define OPHID_DEBUG(fmt, args ...)
|
||||
#define OPHID_TRACE(fmt, args ...)
|
||||
|
@ -94,6 +94,9 @@ void ConnectionDiagram::setupGraphicsScene()
|
||||
case VehicleConfigurationSource::CONTROLLER_NANO:
|
||||
elementsToShow << "controller-nano";
|
||||
break;
|
||||
case VehicleConfigurationSource::CONTROLLER_SPARKY2:
|
||||
elementsToShow << "controller-sparky2";
|
||||
break;
|
||||
case VehicleConfigurationSource::CONTROLLER_OPLINK:
|
||||
default:
|
||||
elementsToShow << "controller-cc";
|
||||
@ -181,6 +184,9 @@ void ConnectionDiagram::setupGraphicsScene()
|
||||
case VehicleConfigurationSource::CONTROLLER_NANO:
|
||||
prefix = "nano-";
|
||||
break;
|
||||
case VehicleConfigurationSource::CONTROLLER_SPARKY2:
|
||||
prefix = "sparky2-";
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -225,7 +231,8 @@ void ConnectionDiagram::setupGraphicsScene()
|
||||
}
|
||||
}
|
||||
|
||||
if (m_configSource->getInputType() == VehicleConfigurationSource::INPUT_SBUS) {
|
||||
if ((m_configSource->getInputType() == VehicleConfigurationSource::INPUT_SBUS) &&
|
||||
(m_configSource->getControllerType() != VehicleConfigurationSource::CONTROLLER_SPARKY2)) {
|
||||
prefix = QString("flexi-%1").arg(prefix);
|
||||
}
|
||||
switch (m_configSource->getGpsType()) {
|
||||
|
@ -39,11 +39,14 @@ void AirSpeedPage::initializePage(VehicleConfigurationSource *settings)
|
||||
{
|
||||
// Enable all
|
||||
setItemDisabled(-1, false);
|
||||
if (settings->getInputType() == VehicleConfigurationSource::INPUT_SBUS ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_DSM ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_SRXL ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_HOTT_SUMD ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_EXBUS ||
|
||||
|
||||
bool isSparky = (getWizard()->getControllerType() == SetupWizard::CONTROLLER_SPARKY2);
|
||||
|
||||
if ((!isSparky && (settings->getInputType() == VehicleConfigurationSource::INPUT_SBUS ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_DSM ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_SRXL ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_HOTT_SUMD ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_EXBUS)) ||
|
||||
settings->getGpsType() == VehicleConfigurationSource::GPS_UBX_FLEXI_I2CMAG) {
|
||||
// Disable non estimated sensors if ports are taken by receivers or I2C Mag
|
||||
setItemDisabled(VehicleConfigurationSource::AIRSPEED_EAGLETREE, true);
|
||||
@ -70,6 +73,7 @@ void AirSpeedPage::setupSelection(Selection *selection)
|
||||
"software estimation technique and the other two utilize hardware sensors.\n\n"
|
||||
"Note: if previously selected input combinations use the Flexi-port for input, "
|
||||
"only estimated airspeed will be available.\n\n"));
|
||||
|
||||
selection->addItem(tr("Estimated"),
|
||||
tr("This option uses an intelligent estimation algorithm which utilizes the INS/GPS "
|
||||
"to estimate wind speed and subtract it from ground speed obtained from the GPS.\n\n"
|
||||
|
@ -117,6 +117,9 @@ SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType()
|
||||
case 0x0905:
|
||||
return SetupWizard::CONTROLLER_NANO;
|
||||
|
||||
case 0x9201:
|
||||
return SetupWizard::CONTROLLER_SPARKY2;
|
||||
|
||||
default:
|
||||
return SetupWizard::CONTROLLER_UNKNOWN;
|
||||
}
|
||||
@ -139,6 +142,7 @@ void ControllerPage::setupBoardTypes()
|
||||
ui->boardTypeCombo->addItem(tr("OpenPilot OPLink Radio Modem"), SetupWizard::CONTROLLER_OPLINK);
|
||||
ui->boardTypeCombo->addItem(tr("OpenPilot DiscoveryF4"), SetupWizard::CONTROLLER_DISCOVERYF4);
|
||||
ui->boardTypeCombo->addItem(tr("OpenPilot Nano"), SetupWizard::CONTROLLER_NANO);
|
||||
ui->boardTypeCombo->addItem(tr("TauLabs Sparky 2.0"), SetupWizard::CONTROLLER_SPARKY2);
|
||||
}
|
||||
|
||||
void ControllerPage::setControllerType(SetupWizard::CONTROLLER_TYPE type)
|
||||
@ -225,6 +229,11 @@ void ControllerPage::connectionStatusChanged()
|
||||
ui->boardImg->setPixmap(boardPic.scaled(picSize, Qt::KeepAspectRatio));
|
||||
break;
|
||||
|
||||
case SetupWizard::CONTROLLER_SPARKY2:
|
||||
boardPic.load(":/configgadget/images/sparky2_top.png");
|
||||
ui->boardImg->setPixmap(boardPic.scaled(picSize, Qt::KeepAspectRatio));
|
||||
break;
|
||||
|
||||
default:
|
||||
ui->boardImg->setPixmap(QPixmap());
|
||||
break;
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file EscCalibrationPage.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup EscCalibrationPage
|
||||
@ -61,6 +62,11 @@ EscCalibrationPage::~EscCalibrationPage()
|
||||
delete ui;
|
||||
}
|
||||
|
||||
void EscCalibrationPage::initializePage()
|
||||
{
|
||||
resetAllSecurityCheckboxes();
|
||||
}
|
||||
|
||||
bool EscCalibrationPage::validatePage()
|
||||
{
|
||||
return true;
|
||||
@ -180,9 +186,3 @@ void EscCalibrationPage::securityCheckBoxesToggled()
|
||||
ui->securityCheckBox2->isChecked() &&
|
||||
ui->securityCheckBox3->isChecked());
|
||||
}
|
||||
|
||||
|
||||
void EscCalibrationPage::initializePage()
|
||||
{
|
||||
resetAllSecurityCheckboxes();
|
||||
}
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file biascalibrationpage.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup BiasCalibrationPage
|
||||
@ -42,8 +43,8 @@ class EscCalibrationPage : public AbstractWizardPage {
|
||||
public:
|
||||
explicit EscCalibrationPage(SetupWizard *wizard, QWidget *parent = 0);
|
||||
~EscCalibrationPage();
|
||||
bool validatePage();
|
||||
void initializePage();
|
||||
bool validatePage();
|
||||
|
||||
private slots:
|
||||
void startButtonClicked();
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file escpage.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup EscPage
|
||||
@ -42,6 +43,17 @@ EscPage::~EscPage()
|
||||
delete ui;
|
||||
}
|
||||
|
||||
void EscPage::initializePage()
|
||||
{
|
||||
bool enabled = isSynchOrOneShotAvailable();
|
||||
|
||||
ui->oneshotESCButton->setEnabled(enabled);
|
||||
if (ui->oneshotESCButton->isChecked() && !enabled) {
|
||||
ui->oneshotESCButton->setChecked(false);
|
||||
ui->rapidESCButton->setChecked(true);
|
||||
}
|
||||
}
|
||||
|
||||
bool EscPage::validatePage()
|
||||
{
|
||||
if (ui->oneshotESCButton->isChecked()) {
|
||||
@ -59,18 +71,6 @@ bool EscPage::validatePage()
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void EscPage::initializePage()
|
||||
{
|
||||
bool enabled = isSynchOrOneShotAvailable();
|
||||
|
||||
ui->oneshotESCButton->setEnabled(enabled);
|
||||
if (ui->oneshotESCButton->isChecked() && !enabled) {
|
||||
ui->oneshotESCButton->setChecked(false);
|
||||
ui->rapidESCButton->setChecked(true);
|
||||
}
|
||||
}
|
||||
|
||||
bool EscPage::isSynchOrOneShotAvailable()
|
||||
{
|
||||
bool available = true;
|
||||
@ -98,6 +98,7 @@ bool EscPage::isSynchOrOneShotAvailable()
|
||||
}
|
||||
break;
|
||||
case SetupWizard::CONTROLLER_REVO:
|
||||
case SetupWizard::CONTROLLER_SPARKY2:
|
||||
available = true;
|
||||
break;
|
||||
default:
|
||||
|
@ -40,8 +40,8 @@ class EscPage : public AbstractWizardPage {
|
||||
public:
|
||||
explicit EscPage(SetupWizard *wizard, QWidget *parent = 0);
|
||||
~EscPage();
|
||||
bool validatePage();
|
||||
void initializePage();
|
||||
bool validatePage();
|
||||
|
||||
private:
|
||||
Ui::EscPage *ui;
|
||||
|
@ -39,11 +39,14 @@ void GpsPage::initializePage(VehicleConfigurationSource *settings)
|
||||
{
|
||||
// Enable all
|
||||
setItemDisabled(-1, false);
|
||||
if (settings->getInputType() == VehicleConfigurationSource::INPUT_SBUS ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_DSM ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_HOTT_SUMD ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_EXBUS ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_SRXL) {
|
||||
|
||||
bool isSparky = (getWizard()->getControllerType() == SetupWizard::CONTROLLER_SPARKY2);
|
||||
// All rcinputs are on rcvrport for sparky2, that leaves mainport/flexiport available for gps/auxmag
|
||||
if (!isSparky && (settings->getInputType() == VehicleConfigurationSource::INPUT_SBUS ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_DSM ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_SRXL ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_HOTT_SUMD ||
|
||||
settings->getInputType() == VehicleConfigurationSource::INPUT_EXBUS)) {
|
||||
// Disable GPS+I2C Mag
|
||||
setItemDisabled(VehicleConfigurationSource::GPS_UBX_FLEXI_I2CMAG, true);
|
||||
if (getSelectedItem()->id() == VehicleConfigurationSource::GPS_UBX_FLEXI_I2CMAG) {
|
||||
|
@ -1,14 +1,15 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
****************************************************************************************
|
||||
*
|
||||
* @file inputpage.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup InputPage
|
||||
* @{
|
||||
* @brief
|
||||
*****************************************************************************/
|
||||
***************************************************************************************/
|
||||
/*
|
||||
* 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
|
||||
@ -45,6 +46,17 @@ InputPage::~InputPage()
|
||||
delete ui;
|
||||
}
|
||||
|
||||
void InputPage::initializePage()
|
||||
{
|
||||
bool isSparky2 = (getWizard()->getControllerType() == SetupWizard::CONTROLLER_SPARKY2);
|
||||
|
||||
ui->pwmButton->setEnabled(!isSparky2);
|
||||
if (ui->pwmButton->isChecked() && isSparky2) {
|
||||
ui->pwmButton->setChecked(false);
|
||||
ui->ppmButton->setChecked(true);
|
||||
}
|
||||
}
|
||||
|
||||
bool InputPage::validatePage()
|
||||
{
|
||||
if (ui->pwmButton->isChecked()) {
|
||||
@ -139,6 +151,32 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp
|
||||
}
|
||||
break;
|
||||
}
|
||||
case SetupWizard::CONTROLLER_SPARKY2:
|
||||
{
|
||||
switch (selectedType) {
|
||||
case VehicleConfigurationSource::INPUT_PPM:
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_PPM;
|
||||
|
||||
case VehicleConfigurationSource::INPUT_SBUS:
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_SBUS;
|
||||
|
||||
case VehicleConfigurationSource::INPUT_SRXL:
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_SRXL;
|
||||
|
||||
case VehicleConfigurationSource::INPUT_DSM:
|
||||
// TODO: Handle all of the DSM types ?? Which is most common?
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_DSM;
|
||||
|
||||
case VehicleConfigurationSource::INPUT_HOTT_SUMD:
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_HOTTSUMD;
|
||||
|
||||
case VehicleConfigurationSource::INPUT_EXBUS:
|
||||
return data.SPK2_RcvrPort != HwSettings::SPK2_RCVRPORT_EXBUS;
|
||||
|
||||
default: return true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: return true;
|
||||
}
|
||||
}
|
||||
|
@ -40,6 +40,7 @@ class InputPage : public AbstractWizardPage {
|
||||
public:
|
||||
explicit InputPage(SetupWizard *wizard, QWidget *parent = 0);
|
||||
~InputPage();
|
||||
void initializePage();
|
||||
bool validatePage();
|
||||
|
||||
private:
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file summarypage.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup SummaryPage
|
||||
@ -40,8 +41,8 @@ class SummaryPage : public AbstractWizardPage {
|
||||
public:
|
||||
explicit SummaryPage(SetupWizard *wizard, QWidget *parent = 0);
|
||||
~SummaryPage();
|
||||
bool validatePage();
|
||||
void initializePage();
|
||||
bool validatePage();
|
||||
|
||||
private:
|
||||
Ui::SummaryPage *ui;
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file vehiclepage.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup VehiclePage
|
||||
@ -57,9 +58,3 @@ bool VehiclePage::validatePage()
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void VehiclePage::initializePage()
|
||||
{
|
||||
// ui->fixedwingButton->setEnabled(getWizard()->getControllerType() == SetupWizard::CONTROLLER_REVO ||
|
||||
// getWizard()->getControllerType() == SetupWizard::CONTROLLER_NANO);
|
||||
}
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file vehiclepage.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @addtogroup
|
||||
* @{
|
||||
* @addtogroup VehiclePage
|
||||
@ -41,7 +42,6 @@ public:
|
||||
explicit VehiclePage(SetupWizard *wizard, QWidget *parent = 0);
|
||||
~VehiclePage();
|
||||
bool validatePage();
|
||||
void initializePage();
|
||||
|
||||
private:
|
||||
Ui::VehiclePage *ui;
|
||||
|
File diff suppressed because one or more lines are too long
Before Width: | Height: | Size: 3.4 MiB After Width: | Height: | Size: 4.6 MiB |
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file setupwizard.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup Setup Wizard Plugin
|
||||
@ -98,6 +99,7 @@ int SetupWizard::nextId() const
|
||||
case CONTROLLER_CC:
|
||||
case CONTROLLER_CC3D:
|
||||
case CONTROLLER_REVO:
|
||||
case CONTROLLER_SPARKY2:
|
||||
case CONTROLLER_DISCOVERYF4:
|
||||
return PAGE_INPUT;
|
||||
|
||||
@ -156,6 +158,7 @@ int SetupWizard::nextId() const
|
||||
switch (getControllerType()) {
|
||||
case CONTROLLER_REVO:
|
||||
case CONTROLLER_NANO:
|
||||
case CONTROLLER_SPARKY2:
|
||||
return PAGE_GPS;
|
||||
|
||||
default:
|
||||
@ -168,6 +171,7 @@ int SetupWizard::nextId() const
|
||||
switch (getControllerType()) {
|
||||
case CONTROLLER_REVO:
|
||||
case CONTROLLER_NANO:
|
||||
case CONTROLLER_SPARKY2:
|
||||
return PAGE_GPS;
|
||||
|
||||
default:
|
||||
@ -213,6 +217,7 @@ int SetupWizard::nextId() const
|
||||
case CONTROLLER_CC3D:
|
||||
case CONTROLLER_REVO:
|
||||
case CONTROLLER_NANO:
|
||||
case CONTROLLER_SPARKY2:
|
||||
case CONTROLLER_DISCOVERYF4:
|
||||
switch (getVehicleType()) {
|
||||
case VEHICLE_FIXEDWING:
|
||||
@ -254,6 +259,9 @@ QString SetupWizard::getSummaryText()
|
||||
case CONTROLLER_NANO:
|
||||
summary.append(tr("OpenPilot Nano"));
|
||||
break;
|
||||
case CONTROLLER_SPARKY2:
|
||||
summary.append(tr("TauLabs Sparky 2.0"));
|
||||
break;
|
||||
case CONTROLLER_OPLINK:
|
||||
summary.append(tr("OpenPilot OPLink Radio Modem"));
|
||||
break;
|
||||
@ -427,7 +435,7 @@ QString SetupWizard::getSummaryText()
|
||||
}
|
||||
|
||||
// Show GPS Type
|
||||
if (getControllerType() == CONTROLLER_REVO || getControllerType() == CONTROLLER_NANO) {
|
||||
if (getControllerType() == CONTROLLER_REVO || getControllerType() == CONTROLLER_NANO || getControllerType() == CONTROLLER_SPARKY2) {
|
||||
summary.append("<br>");
|
||||
summary.append("<b>").append(tr("GPS type: ")).append("</b>");
|
||||
switch (getGpsType()) {
|
||||
@ -452,7 +460,8 @@ QString SetupWizard::getSummaryText()
|
||||
}
|
||||
|
||||
// Show Airspeed sensor type
|
||||
if ((getControllerType() == CONTROLLER_REVO || getControllerType() == CONTROLLER_NANO) && getVehicleType() == VEHICLE_FIXEDWING) {
|
||||
if ((getControllerType() == CONTROLLER_REVO || getControllerType() == CONTROLLER_NANO || getControllerType() == CONTROLLER_SPARKY2)
|
||||
&& getVehicleType() == VEHICLE_FIXEDWING) {
|
||||
summary.append("<br>");
|
||||
summary.append("<b>").append(tr("Airspeed Sensor: ")).append("</b>");
|
||||
switch (getAirspeedType()) {
|
||||
|
@ -1,5 +1,5 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
***********************************************************************************
|
||||
*
|
||||
* @file vehicleconfigurationhelper.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
@ -9,7 +9,7 @@
|
||||
* @addtogroup VehicleConfigurationHelper
|
||||
* @{
|
||||
* @brief
|
||||
*****************************************************************************/
|
||||
**********************************************************************************/
|
||||
/*
|
||||
* 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
|
||||
@ -27,7 +27,6 @@
|
||||
*/
|
||||
|
||||
#include "vehicleconfigurationhelper.h"
|
||||
|
||||
#include "extensionsystem/pluginmanager.h"
|
||||
#include "uavobjectutilmanager.h"
|
||||
|
||||
@ -165,7 +164,7 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_SBUS:
|
||||
// We have to set teletry on flexiport since s.bus needs the mainport.
|
||||
// We have to set telemetry on flexiport since s.bus needs the mainport.
|
||||
data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS;
|
||||
data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY;
|
||||
break;
|
||||
@ -187,43 +186,80 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
break;
|
||||
case VehicleConfigurationSource::CONTROLLER_REVO:
|
||||
case VehicleConfigurationSource::CONTROLLER_NANO:
|
||||
case VehicleConfigurationSource::CONTROLLER_SPARKY2:
|
||||
case VehicleConfigurationSource::CONTROLLER_DISCOVERYF4:
|
||||
// Reset all ports to their defaults
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED;
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_DISABLED;
|
||||
data.SPK2_FlexiPort = HwSettings::SPK2_FLEXIPORT_DISABLED;
|
||||
} else {
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED;
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED;
|
||||
}
|
||||
|
||||
// Revo uses inbuilt Modem do not set mainport to be active telemetry link for the Revo
|
||||
// Revo/Sparky2 uses inbuilt Modem do not set mainport to be active telemetry link for Revo/Sparky2
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_DISABLED;
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_MainPort = HwSettings::SPK2_MAINPORT_DISABLED;
|
||||
} else {
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_TELEMETRY;
|
||||
}
|
||||
|
||||
switch (m_configSource->getInputType()) {
|
||||
case VehicleConfigurationSource::INPUT_PWM:
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
// this should not happen, sparky2 does not allow pwm
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_DISABLED;
|
||||
} else {
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM;
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_PPM:
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_PPM;
|
||||
} else {
|
||||
data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM;
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_SBUS:
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS;
|
||||
// We have to set telemetry on flexiport since s.bus needs the mainport on all but Revo.
|
||||
if (m_configSource->getControllerType() != VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_SBUS;
|
||||
} else {
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS;
|
||||
// We have to set telemetry to flexiport on all except Revo (and except Sparky2) since s.bus needs mainport.
|
||||
if (m_configSource->getControllerType() != VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_DSM:
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_DSM;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM;
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_SRXL:
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_SRXL;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_SRXL;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_SRXL;
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_HOTT_SUMD:
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_HOTTSUMD;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_HOTTSUMD;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_HOTTSUMD;
|
||||
}
|
||||
break;
|
||||
case VehicleConfigurationSource::INPUT_EXBUS:
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_EXBUS;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_RcvrPort = HwSettings::SPK2_RCVRPORT_EXBUS;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_EXBUS;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
@ -233,10 +269,16 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = 1;
|
||||
data.GPSSpeed = HwSettings::GPSSPEED_57600;
|
||||
|
||||
if (m_configSource->getInputType() == VehicleConfigurationSource::INPUT_SBUS) {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_GPS;
|
||||
// if using GPS and SBUS on Revo or Nano, we must use FlexiPort for GPS
|
||||
// since we must use MainPort for SBUS
|
||||
if (m_configSource->getControllerType() != VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
if (m_configSource->getInputType() == VehicleConfigurationSource::INPUT_SBUS) {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_GPS;
|
||||
} else {
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_GPS;
|
||||
}
|
||||
} else {
|
||||
data.RM_MainPort = HwSettings::RM_MAINPORT_GPS;
|
||||
data.SPK2_MainPort = HwSettings::SPK2_MAINPORT_GPS;
|
||||
}
|
||||
|
||||
GPSSettings *gpsSettings = GPSSettings::GetInstance(m_uavoManager);
|
||||
@ -280,12 +322,17 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
}
|
||||
case VehicleConfigurationSource::GPS_UBX_FLEXI_I2CMAG:
|
||||
{
|
||||
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
|
||||
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE;
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
AuxMagSettings *magSettings = AuxMagSettings::GetInstance(m_uavoManager);
|
||||
Q_ASSERT(magSettings);
|
||||
AuxMagSettings::DataFields magsData = magSettings->getData();
|
||||
|
||||
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
|
||||
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.SPK2_FlexiPort = HwSettings::SPK2_FLEXIPORT_I2C;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
}
|
||||
magsData.Type = AuxMagSettings::TYPE_FLEXI;
|
||||
magsData.Usage = AuxMagSettings::USAGE_AUXONLY;
|
||||
magSettings->setData(magsData);
|
||||
@ -316,12 +363,22 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
break;
|
||||
case VehicleConfigurationSource::AIRSPEED_EAGLETREE:
|
||||
data.OptionalModules[HwSettings::OPTIONALMODULES_AIRSPEED] = 1;
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
// sparky2: put I2C airspeed on flexiport, but it could be put on i2cport
|
||||
data.SPK2_FlexiPort = HwSettings::SPK2_FLEXIPORT_I2C;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
}
|
||||
airspeedData.AirspeedSensorType = AirspeedSettings::AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3;
|
||||
break;
|
||||
case VehicleConfigurationSource::AIRSPEED_MS4525:
|
||||
data.OptionalModules[HwSettings::OPTIONALMODULES_AIRSPEED] = 1;
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
// sparky2: put I2C airspeed on flexiport, but it could be put on i2cport
|
||||
data.SPK2_FlexiPort = HwSettings::SPK2_FLEXIPORT_I2C;
|
||||
} else {
|
||||
data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_I2C;
|
||||
}
|
||||
airspeedData.AirspeedSensorType = AirspeedSettings::AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO;
|
||||
break;
|
||||
default:
|
||||
@ -484,7 +541,8 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
|
||||
// Servo always on channel 4
|
||||
data.BankUpdateFreq[0] = escFrequence;
|
||||
data.BankMode[0] = bankMode;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO
|
||||
|| m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.BankUpdateFreq[1] = escFrequence;
|
||||
data.BankMode[1] = bankMode;
|
||||
data.BankUpdateFreq[2] = servoFrequence;
|
||||
@ -502,7 +560,8 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
|
||||
data.BankMode[0] = bankMode;
|
||||
data.BankUpdateFreq[1] = escFrequence;
|
||||
data.BankMode[1] = bankMode;
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO
|
||||
|| m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
data.BankUpdateFreq[2] = escFrequence;
|
||||
data.BankMode[2] = bankMode;
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_NANO) {
|
||||
@ -566,7 +625,8 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
|
||||
if (i == 1) {
|
||||
data.BankUpdateFreq[i] = escFrequence;
|
||||
}
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO
|
||||
|| m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
// Motor output4, bank3
|
||||
if (i == 2) {
|
||||
data.BankUpdateFreq[i] = escFrequence;
|
||||
@ -611,7 +671,8 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
|
||||
if (i == 1) {
|
||||
data.BankUpdateFreq[i] = escFrequence;
|
||||
}
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) {
|
||||
} else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO
|
||||
|| m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_SPARKY2) {
|
||||
// Motor output4, bank3
|
||||
if (i == 1) {
|
||||
data.BankUpdateFreq[i] = escFrequence;
|
||||
@ -718,6 +779,7 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
|
||||
}
|
||||
case VehicleConfigurationSource::CONTROLLER_REVO:
|
||||
case VehicleConfigurationSource::CONTROLLER_NANO:
|
||||
case VehicleConfigurationSource::CONTROLLER_SPARKY2:
|
||||
{
|
||||
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager);
|
||||
Q_ASSERT(revolutionCalibration);
|
||||
@ -1357,42 +1419,43 @@ void VehicleConfigurationHelper::setupHexaCopter()
|
||||
channels[0].throttle2 = 0;
|
||||
channels[0].roll = 100;
|
||||
channels[0].pitch = 25;
|
||||
channels[0].yaw = -66;
|
||||
channels[0].yaw = -66;
|
||||
|
||||
|
||||
channels[1].type = MIXER_TYPE_MOTOR;
|
||||
channels[1].throttle1 = 100;
|
||||
channels[1].throttle2 = 0;
|
||||
channels[1].roll = 100;
|
||||
channels[1].pitch = 25;
|
||||
channels[1].yaw = 66;
|
||||
channels[1].yaw = 66;
|
||||
|
||||
channels[2].type = MIXER_TYPE_MOTOR;
|
||||
channels[2].throttle1 = 100;
|
||||
channels[2].throttle2 = 0;
|
||||
channels[2].roll = -100;
|
||||
channels[2].pitch = 25;
|
||||
channels[2].yaw = -66;
|
||||
channels[2].yaw = -66;
|
||||
|
||||
channels[3].type = MIXER_TYPE_MOTOR;
|
||||
channels[3].throttle1 = 100;
|
||||
channels[3].throttle2 = 0;
|
||||
channels[3].roll = -100;
|
||||
channels[3].pitch = 25;
|
||||
channels[3].yaw = 66;
|
||||
channels[3].yaw = 66;
|
||||
|
||||
channels[4].type = MIXER_TYPE_MOTOR;
|
||||
channels[4].throttle1 = 100;
|
||||
channels[4].throttle2 = 0;
|
||||
channels[4].roll = 0;
|
||||
channels[4].pitch = -50;
|
||||
channels[4].yaw = -66;
|
||||
channels[4].yaw = -66;
|
||||
|
||||
channels[5].type = MIXER_TYPE_MOTOR;
|
||||
channels[5].throttle1 = 100;
|
||||
channels[5].throttle2 = 0;
|
||||
channels[5].roll = 0;
|
||||
channels[5].pitch = -50;
|
||||
channels[5].yaw = 66;
|
||||
channels[5].yaw = 66;
|
||||
|
||||
guiSettings.multi.VTOLMotorNW = 1;
|
||||
guiSettings.multi.VTOLMotorW = 2;
|
||||
@ -1837,6 +1900,7 @@ void VehicleConfigurationHelper::setupOctoCopter()
|
||||
guiSettings.multi.VTOLMotorE = 3;
|
||||
guiSettings.multi.VTOLMotorSE = 4;
|
||||
guiSettings.multi.VTOLMotorS = 5;
|
||||
|
||||
guiSettings.multi.VTOLMotorSW = 6;
|
||||
guiSettings.multi.VTOLMotorW = 7;
|
||||
guiSettings.multi.VTOLMotorNW = 8;
|
||||
|
@ -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_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_SPARKY2, 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,
|
||||
|
@ -247,6 +247,11 @@
|
||||
<string>Nano</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>Sparky2</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
|
@ -9,9 +9,9 @@ public:
|
||||
QString gitTag;
|
||||
QByteArray fwHash;
|
||||
QByteArray uavoHash;
|
||||
int boardType;
|
||||
int boardRevision;
|
||||
static QString idToBoardName(int id)
|
||||
quint8 boardType;
|
||||
quint8 boardRevision;
|
||||
static QString idToBoardName(quint16 id)
|
||||
{
|
||||
switch (id) {
|
||||
case 0x0101:
|
||||
@ -42,12 +42,12 @@ public:
|
||||
|
||||
break;
|
||||
case 0x0901:
|
||||
// Revolution
|
||||
// old unreleased Revolution prototype
|
||||
return QString("Revolution");
|
||||
|
||||
break;
|
||||
case 0x0903:
|
||||
// Revo Mini
|
||||
// Revo also known as Revo Mini
|
||||
return QString("Revolution");
|
||||
|
||||
break;
|
||||
@ -59,6 +59,10 @@ public:
|
||||
// Nano
|
||||
return QString("RevoNano");
|
||||
|
||||
case 0x9201:
|
||||
// Sparky 2.0
|
||||
return QString("Sparky2");
|
||||
|
||||
default:
|
||||
return QString("");
|
||||
|
||||
|
@ -469,8 +469,8 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip
|
||||
|
||||
// TODO: check platform compatibility
|
||||
QByteArray targetPlatform = desc.mid(12, 2);
|
||||
struc.boardType = (int)targetPlatform.at(0);
|
||||
struc.boardRevision = (int)targetPlatform.at(1);
|
||||
struc.boardType = (quint8)targetPlatform.at(0);
|
||||
struc.boardRevision = (quint8)targetPlatform.at(1);
|
||||
struc.fwHash.clear();
|
||||
struc.fwHash = desc.mid(40, 20);
|
||||
struc.uavoHash.clear();
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file devicewidget.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup Uploader Serial and USB Uploader Plugin
|
||||
@ -100,14 +101,21 @@ void DeviceWidget::populate()
|
||||
devicePic.load(":/uploader/images/gcs-board-cc3d.png");
|
||||
break;
|
||||
case 0x0903:
|
||||
// Revo
|
||||
devicePic.load(":/uploader/images/gcs-board-revo.png");
|
||||
break;
|
||||
case 0x0904:
|
||||
// DiscoveryF4Bare
|
||||
devicePic.load(":/uploader/images/gcs-board-revo.png");
|
||||
break;
|
||||
case 0x0905:
|
||||
// Nano
|
||||
devicePic.load(":/uploader/images/gcs-board-nano.png");
|
||||
break;
|
||||
case 0x9201:
|
||||
// Sparky2
|
||||
devicePic.load(":/uploader/images/gcs-board-sparky2.png");
|
||||
break;
|
||||
default:
|
||||
// Clear
|
||||
devicePic.load("");
|
||||
@ -195,7 +203,7 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc)
|
||||
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
|
||||
}
|
||||
|
||||
myDevice->lblBrdName->setText(deviceDescriptorStruct::idToBoardName(onBoardDescription.boardType << 8 | onBoardDescription.boardRevision));
|
||||
myDevice->lblBrdName->setText(deviceDescriptorStruct::idToBoardName(((quint16)onBoardDescription.boardType << 8) | onBoardDescription.boardRevision));
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -219,7 +227,7 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc)
|
||||
myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/warning.svg"));
|
||||
myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build"));
|
||||
}
|
||||
myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision));
|
||||
myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(((quint16)LoadedDescription.boardType << 8) | LoadedDescription.boardRevision));
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -363,13 +371,16 @@ void DeviceWidget::uploadFirmware()
|
||||
// Now do sanity checking:
|
||||
// - 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
|
||||
(board == 0x902 && firmwareBoard == 0x903)) { // RevoMini1 supporetd by RevoMini2 firmware
|
||||
int firmwareBoard = ((quint16)(quint8)desc.at(12) << 8) + (quint16)(quint8)desc.at(13);
|
||||
if ((board == 0x0401 && firmwareBoard == 0x0402) ||
|
||||
(board == 0x0901 && firmwareBoard == 0x0902) || // L3GD20 revo supports Revolution firmware
|
||||
(board == 0x0902 && firmwareBoard == 0x0903) || // RevoMini1 supported by RevoMini2 firmware
|
||||
(board == 0x0b01 && firmwareBoard == 0x9201)) { // Sparky2 before and after TL BL compatibility change
|
||||
// These firmwares are designed to be backwards compatible
|
||||
} else if (firmwareBoard != board) {
|
||||
status("Error: firmware does not match board", STATUSICON_FAIL);
|
||||
char buf[100];
|
||||
sprintf(buf, "Error: Device ID: firmware 0x%x does not match board 0x%x", firmwareBoard, board);
|
||||
status(buf, STATUSICON_FAIL);
|
||||
updateButtons(true);
|
||||
return;
|
||||
}
|
||||
|
BIN
ground/gcs/src/plugins/uploader/images/gcs-board-sparky2.png
Normal file
BIN
ground/gcs/src/plugins/uploader/images/gcs-board-sparky2.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 218 KiB |
@ -1084,11 +1084,12 @@ 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
|
||||
#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
|
||||
#define BOARD_ID_SPARKY2 0x92
|
||||
|
||||
/**
|
||||
Gets the type of board connected
|
||||
@ -1117,6 +1118,9 @@ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum)
|
||||
case BOARD_ID_REVO: // Revo board
|
||||
brdType = eBoardRevo;
|
||||
break;
|
||||
case BOARD_ID_SPARKY2: // Sparky2 board
|
||||
brdType = eBoardSparky2;
|
||||
break;
|
||||
}
|
||||
return brdType;
|
||||
}
|
||||
|
@ -60,6 +60,7 @@ enum Status {
|
||||
};
|
||||
|
||||
enum Actions {
|
||||
actionNone,
|
||||
actionProgram,
|
||||
actionProgramAndVerify,
|
||||
actionDownload,
|
||||
@ -94,12 +95,13 @@ enum eBoardType {
|
||||
eBoardPip = 3,
|
||||
eBoardCC = 4,
|
||||
eBoardRevo = 9,
|
||||
eBoardSparky2 = 0x92,
|
||||
};
|
||||
|
||||
struct device {
|
||||
int ID;
|
||||
quint16 ID;
|
||||
quint32 FW_CRC;
|
||||
int BL_Version;
|
||||
quint8 BL_Version;
|
||||
int SizeOfDesc;
|
||||
quint32 SizeOfCode;
|
||||
bool Readable;
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file runningdevicewidget.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @addtogroup GCSPlugins GCS Plugins
|
||||
* @{
|
||||
* @addtogroup Uploader Serial and USB Uploader Plugin
|
||||
@ -86,11 +87,20 @@ void RunningDeviceWidget::populate()
|
||||
devicePic.load(":/uploader/images/gcs-board-cc3d.png");
|
||||
break;
|
||||
case 0x0903:
|
||||
// Revo
|
||||
// fall through to DF4B
|
||||
case 0x0904:
|
||||
// DiscoveryF4Bare
|
||||
devicePic.load(":/uploader/images/gcs-board-revo.png");
|
||||
break;
|
||||
case 0x0905:
|
||||
// Nano
|
||||
devicePic.load(":/uploader/images/gcs-board-nano.png");
|
||||
break;
|
||||
case 0x9201:
|
||||
// Sparky2
|
||||
devicePic.load(":/uploader/images/gcs-board-sparky2.png");
|
||||
break;
|
||||
default:
|
||||
// Clear
|
||||
devicePic.load("");
|
||||
|
@ -19,5 +19,6 @@
|
||||
<file>images/gcs-board-oplink.png</file>
|
||||
<file>images/gcs-board-revo.png</file>
|
||||
<file>images/gcs-board-nano.png</file>
|
||||
<file>images/gcs-board-sparky2.png</file>
|
||||
</qresource>
|
||||
</RCC>
|
||||
|
@ -751,28 +751,31 @@ bool UploaderGadgetWidget::autoUpdate(bool erase)
|
||||
QString filename;
|
||||
emit progressUpdate(LOADING_FW, QVariant());
|
||||
switch (m_dfu->devices[0].ID) {
|
||||
case 0x301:
|
||||
case 0x0301:
|
||||
filename = "fw_oplinkmini";
|
||||
break;
|
||||
case 0x401:
|
||||
case 0x402:
|
||||
case 0x0401:
|
||||
case 0x0402:
|
||||
filename = "fw_coptercontrol";
|
||||
break;
|
||||
case 0x501:
|
||||
case 0x0501:
|
||||
filename = "fw_osd";
|
||||
break;
|
||||
case 0x902:
|
||||
case 0x0902:
|
||||
filename = "fw_revoproto";
|
||||
break;
|
||||
case 0x903:
|
||||
case 0x0903:
|
||||
filename = "fw_revolution";
|
||||
break;
|
||||
case 0x904:
|
||||
case 0x0904:
|
||||
filename = "fw_discoveryf4bare";
|
||||
break;
|
||||
case 0x905:
|
||||
case 0x0905:
|
||||
filename = "fw_revonano";
|
||||
break;
|
||||
case 0x9201:
|
||||
filename = "fw_sparky2";
|
||||
break;
|
||||
default:
|
||||
emit progressUpdate(FAILURE, QVariant(tr("Unknown board id '0x%1'").arg(QString::number(m_dfu->devices[0].ID, 16))));
|
||||
emit autoUpdateFailed();
|
||||
@ -1020,7 +1023,8 @@ void UploaderGadgetWidget::startAutoUpdateErase()
|
||||
UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
|
||||
int id = utilMngr->getBoardModel();
|
||||
|
||||
if (id == 0x905) {
|
||||
// reset if Nano
|
||||
if (id == 0x0905) {
|
||||
systemReset();
|
||||
}
|
||||
}
|
||||
|
@ -194,6 +194,13 @@ void UsageTrackerPlugin::collectUsageParameters(QMap<QString, QString> ¶mete
|
||||
parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "RM_MainPort");
|
||||
parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "RM_FlexiPort");
|
||||
parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm");
|
||||
} else if ((boardModel & 0xff00) == 0x9200) {
|
||||
// Sparky2
|
||||
parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "SPK2_RcvrPort");
|
||||
parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "SPK2_MainPort");
|
||||
parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "SPK2_FlexiPort");
|
||||
parameters["conf_iport"] = getUAVFieldValue(objManager, "HwSettings", "SPK2_I2CPort");
|
||||
parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm");
|
||||
}
|
||||
|
||||
parameters["conf_uport"] = getUAVFieldValue(objManager, "HwSettings", "USB_HIDPort");
|
||||
|
@ -14,6 +14,9 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415c", GOTO="op_ru
|
||||
# OpenPilot Revolution board
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415e", GOTO="op_rules"
|
||||
|
||||
# Taulabs Sparky2 board
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="41d0", GOTO="op_rules"
|
||||
|
||||
# Other OpenPilot reserved pids
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415d", GOTO="op_rules"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4194", GOTO="op_rules"
|
||||
|
@ -16,6 +16,12 @@
|
||||
limits="%0905NE:PPM+PWM:PPM+Telemetry:Telemetry:ComBridge;"/>
|
||||
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,DebugConsole,ComBridge,OsdHk,MSP" defaultvalue="Disabled"/>
|
||||
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,SRXL,DebugConsole,ComBridge,OsdHk,MSP" defaultvalue="Disabled"/>
|
||||
|
||||
<field name="SPK2_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PPM,S.Bus,DSM,SRXL,EX.Bus,HoTT SUMD,HoTT SUMH" defaultvalue="PPM"/>
|
||||
<field name="SPK2_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,DSM,DebugConsole,ComBridge,OsdHk,MSP" defaultvalue="Disabled"/>
|
||||
<field name="SPK2_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,SRXL,DebugConsole,ComBridge,OsdHk,MSP" defaultvalue="Disabled"/>
|
||||
<field name="SPK2_I2CPort" units="function" type="enum" elements="1" options="Disabled,I2C" defaultvalue="Disabled"/>
|
||||
|
||||
<field name="TelemetrySpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
|
||||
<field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400" defaultvalue="57600"/>
|
||||
<field name="ComUsbBridgeSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
|
||||
|
@ -3,7 +3,7 @@
|
||||
<description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
|
||||
<field name="ChannelGroups" units="Channel Group" type="enum"
|
||||
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"
|
||||
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,EX.Bus,HoTT,SRXL,GCS,OPLink,None" defaultvalue="None"/>
|
||||
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),DSM (RcvrPort),S.Bus,EX.Bus,HoTT,SRXL,GCS,OPLink,None" defaultvalue="None"/>
|
||||
<field name="ChannelNumber" units="channel" type="uint8" defaultvalue="0"
|
||||
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
|
||||
<field name="ChannelMin" units="us" type="int16" defaultvalue="1000"
|
||||
|
@ -2,7 +2,7 @@
|
||||
<object name="ReceiverActivity" singleinstance="true" settings="false" category="System">
|
||||
<description>Monitors which receiver channels have been active within the last second.</description>
|
||||
<field name="ActiveGroup" units="Channel Group" type="enum" elements="1"
|
||||
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,EX.Bus,HoTT,SRXL,GCS,OPLink,None"
|
||||
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),DSM (RcvrPort),S.Bus,EX.Bus,HoTT,SRXL,GCS,OPLink,None"
|
||||
defaultvalue="None"/>
|
||||
<field name="ActiveChannel" units="channel" type="uint8" elements="1"
|
||||
defaultvalue="255"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user