diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 4f7db7c..621c6a2 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1413,7 +1413,7 @@ void handleBlackbox(void)
static bool canUseBlackboxWithCurrentConfiguration(void)
{
- return feature(FEATURE_BLACKBOX);
+ return false;//feature(FEATURE_BLACKBOX);
}
/**
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 53b888a..890d76d 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -399,11 +399,7 @@ static void resetConf(void)
setControlRateProfile(0);
masterConfig.version = EEPROM_CONF_VERSION;
-#if defined(SPRACINGF3)
- masterConfig.mixerMode = MIXER_FLYING_WING;
-#else
masterConfig.mixerMode = MIXER_QUADX;
-#endif
featureClearAll();
//////// Nueva Líneas
featureSet(FEATURE_SERVO_TILT);
@@ -675,7 +671,7 @@ static void resetConf(void)
/////////////////////////////
//TRACKER CONFIG
- featureSet(FEATURE_SOFTSERIAL);
+ //featureSet(FEATURE_SOFTSERIAL);
//Serial 1
masterConfig.serialConfig.portConfigs[0].identifier = SERIAL_PORT_USART1;
@@ -685,6 +681,7 @@ static void resetConf(void)
masterConfig.serialConfig.portConfigs[1].identifier = SERIAL_PORT_USART2;
masterConfig.serialConfig.portConfigs[1].gps_baudrateIndex = BAUD_9600;
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_GPS;
+
//Softserial 1
/*masterConfig.serialConfig.portConfigs[2].identifier = SERIAL_PORT_SOFTSERIAL1;
masterConfig.serialConfig.portConfigs[2].telemetry_baudrateIndex = BAUD_9600;
@@ -703,6 +700,7 @@ static void resetConf(void)
masterConfig.max_pid_divider = 15;
//PAN
+ masterConfig.pan_pin = 0;
masterConfig.pan0 = 1500;
pwmPan0 = masterConfig.pan0;
masterConfig.pan_calibration_pulse = 1400;
@@ -719,19 +717,20 @@ static void resetConf(void)
masterConfig.mag_calibrated = 0;
//TILT
+ masterConfig.tilt_pin = 1;
masterConfig.tilt0 = 500;
masterConfig.tilt90 = 1400;
masterConfig.tilt_max_angle = 0;
//EASING
- featureSet(FEATURE_EASING);
+ //featureSet(FEATURE_EASING);
masterConfig.easing = 2 ;
masterConfig.easing_steps = 60;
masterConfig.easing_min_angle = 4;
masterConfig.easing_millis = 15;
//NOPID feature by default
- featureSet(FEATURE_NOPID);
+ //featureSet(FEATURE_NOPID);
masterConfig.nopid_min_delta = 0.2;
masterConfig.nopid_max_speed = 200;
masterConfig.nopid_map_angle = 90;
@@ -744,7 +743,7 @@ static void resetConf(void)
masterConfig.telemetry_provider = 0;
//Display
- featureSet(FEATURE_DISPLAY);
+ //featureSet(FEATURE_DISPLAY);
//GPS
featureClear(FEATURE_GPS);
@@ -764,19 +763,22 @@ static void resetConf(void)
// FILTERS
masterConfig.max_speed_filter = 0;
//VBAT
- featureSet(FEATURE_VBAT);
+ //featureSet(FEATURE_VBAT);
//BUTTONS
masterConfig.min_logic_level=60;
//TELEMETRY OUT
- featureSet(FEATURE_TELEMETRY);
+ //featureSet(FEATURE_TELEMETRY);
//RSSI
//featureSet(FEATURE_RSSI_ADC);
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_zoom = 35;
+ //ALTITUDE
+ masterConfig.altitude_priority = 0;
+
/////////////////////////////
// copy first profile into remaining profile
diff --git a/src/main/config/config.h b/src/main/config/config.h
index ac827d2..ede5c9d 100755
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -42,6 +42,7 @@ typedef enum {
FEATURE_RX_PPM = 1 << 15,*/
} features_e;
+uint8_t pwmPanPin;
uint16_t pwmPan0;
uint16_t pwmPanCalibrationPulse;
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 40ff3b7..281a786 100755
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -112,12 +112,14 @@ typedef struct master_t {
uint16_t max_pid_gain;
uint8_t max_pid_divider;
// EASING
+ uint8_t pan_pin;
uint16_t pan0;
uint8_t pan0_calibrated;
uint8_t mag_calibrated;
uint8_t min_pan_speed;
int16_t offset;
int8_t offset_trim;
+ uint8_t tilt_pin;
uint16_t tilt0;
uint16_t tilt90;
uint8_t tilt_max_angle;
@@ -147,6 +149,7 @@ typedef struct master_t {
uint8_t update_home_by_local_gps;
uint16_t pan_calibration_pulse;
uint8_t max_speed_filter;
+ uint8_t altitude_priority;
} master_t;
extern master_t masterConfig;
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index fa4e977..d92d130 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -78,10 +78,10 @@ enum {
#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(PORT103R)
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
- PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
- PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
- PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
- PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
+ PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed
+ PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed
+ PWM11 | (MAP_TO_SERVO_OUTPUT << 8),
+ PWM12 | (MAP_TO_SERVO_OUTPUT << 8),
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
@@ -329,10 +329,10 @@ static const uint16_t airPWM[] = {
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
- PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
- PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
- PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
- PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
+ PWM9 | (MAP_TO_SERVO_OUTPUT << 8),
+ PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
+ PWM11 | (MAP_TO_SERVO_OUTPUT << 8),
+ PWM12 | (MAP_TO_SERVO_OUTPUT << 8),
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index d32a93a..aa8a394 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -185,7 +185,7 @@ void calculate_Gtune(uint8_t axis)
}
int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
-#ifdef BLACKBOX
+/*#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_gtuneCycleResult_t eventData;
@@ -194,7 +194,7 @@ void calculate_Gtune(uint8_t axis)
eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10
blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
}
-#endif
+#endif*/
if (floatPID) {
pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 594d89a..804b1cd 100755
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -76,13 +76,13 @@ void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction,
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
- if (feature(FEATURE_BLACKBOX)) {
+/* if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
- }
+ }*/
#endif
}
@@ -91,13 +91,13 @@ void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunc
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
- if (feature(FEATURE_BLACKBOX)) {
+ /*if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
- }
+ }*/
#endif
}
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 0a7f9a1..fc212e6 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -354,6 +354,10 @@ static const char * const lookupTabletelemetryProvider[] = {
"NONE", "DIY_GPS","INAV","APM10"
};
+static const char * const lookupTableAltitudePriority[] = {
+ "BARO","GPS"
+};
+
static const char * const lookupTabletelemetryHome[] = {
"DEFAULT", "AUTO"
};
@@ -405,6 +409,7 @@ typedef enum {
TABLE_UNIT,
TABLE_ALIGNMENT,
TABLE_TELEMETRY_PROVIDER,
+ TABLE_ALTITUDE_PRIORITY,
TABLE_TELEMETRY_HOME,
#ifdef GPS
TABLE_GPS_PROVIDER,
@@ -424,6 +429,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) },
{ lookupTableAlignment, sizeof(lookupTableAlignment) / sizeof(char *) },
{ lookupTabletelemetryProvider, sizeof(lookupTabletelemetryProvider) / sizeof(char *) },
+ { lookupTableAltitudePriority, sizeof(lookupTableAltitudePriority) / sizeof(char *) },
{ lookupTabletelemetryHome, sizeof(lookupTabletelemetryHome) / sizeof(char *) },
#ifdef GPS
{ lookupTableGPSProvider, sizeof(lookupTableGPSProvider) / sizeof(char *) },
@@ -551,7 +557,7 @@ const clivalue_t valueTable[] = {
*/
#endif
-/* { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } },
+/* { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } },
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} },
{ "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.telemetry_switch, .config.lookup = { TABLE_OFF_ON } },*/
@@ -568,23 +574,21 @@ const clivalue_t valueTable[] = {
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
-// { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
-// { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
-// { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
-// { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
-/*
+// { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
+// { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
+// { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
+// { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
- { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
-*/ { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
-/*
+// { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
+ { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
+
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.rollDegrees, .config.minmax = { -180, 360 } },
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDegrees, .config.minmax = { -180, 360 } },
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
-
- { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
+// { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_lpf, .config.minmax = { 0, 256 } },
- { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
+/* { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, .config.minmax = { 100, 1000 } },
{ "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, .config.minmax = { 100, 1000 } },
@@ -720,13 +724,15 @@ const clivalue_t valueTable[] = {
{ "max_pid_accumulator", VAR_UINT16 | TRACKER_VALUE, &masterConfig.max_pid_accumulator, .config.minmax = { 0, 50000 } },
{ "max_pid_gain", VAR_UINT16 | TRACKER_VALUE, &masterConfig.max_pid_gain, .config.minmax = { 0, 5000 } },
{ "pid_divider", VAR_UINT8 | TRACKER_VALUE, &masterConfig.max_pid_divider, .config.minmax = { 0, 255 } },
+ { "pan_pin", VAR_UINT8 | TRACKER_VALUE, &masterConfig.pan_pin, .config.minmax = { 0, 7 } },
{ "pan0", VAR_UINT16 | TRACKER_VALUE, &masterConfig.pan0, .config.minmax = { 0, 3000 } },
{ "pan0_calibrated", VAR_UINT8 | TRACKER_VALUE, &masterConfig.pan0_calibrated, .config.minmax = { 0, 1 } },
{ "pan_calibration_pulse", VAR_UINT16 | TRACKER_VALUE, &masterConfig.pan_calibration_pulse, .config.minmax = { 1, 1500 } },
- { "min_pan_speed", VAR_INT8 | TRACKER_VALUE, &masterConfig.min_pan_speed, .config.minmax = { 0, 100 } },
+ { "min_pan_speed", VAR_UINT8 | TRACKER_VALUE, &masterConfig.min_pan_speed, .config.minmax = { 0, 100 } },
{ "offset", VAR_INT16 | TRACKER_VALUE, &masterConfig.offset, .config.minmax = { -360, 360 } },
{ "offset_trim", VAR_INT8 | TRACKER_VALUE, &masterConfig.offset_trim, .config.minmax = { -20, 20 } },
{ "init_servos", VAR_UINT8 | TRACKER_VALUE, &masterConfig.init_servos, .config.minmax = { 0, 1 } },
+ { "tilt_pin", VAR_UINT8 | TRACKER_VALUE, &masterConfig.tilt_pin, .config.minmax = { 0, 7 } },
{ "tilt0", VAR_UINT16 | TRACKER_VALUE, &masterConfig.tilt0, .config.minmax = { 0, 3000 } },
{ "tilt90", VAR_UINT16 | TRACKER_VALUE, &masterConfig.tilt90, .config.minmax = { 0, 3000 } },
{ "tilt_max_angle", VAR_UINT8 | TRACKER_VALUE, &masterConfig.tilt_max_angle, .config.minmax = { 0, 90 } },
@@ -757,7 +763,8 @@ const clivalue_t valueTable[] = {
{ "eps_interpolation", VAR_UINT8 | TRACKER_VALUE | MODE_LOOKUP, &masterConfig.eps_interpolation, .config.lookup = { TABLE_OFF_ON } },
{ "eps_interpolation_points", VAR_UINT8 | TRACKER_VALUE, &masterConfig.eps_interpolation_points, .config.minmax = { 2, 10 } },
{ "update_home_by_local_gps", VAR_UINT8 | TRACKER_VALUE | MODE_LOOKUP, &masterConfig.update_home_by_local_gps, .config.lookup = { TABLE_OFF_ON } },
- { "max_speed_filter", VAR_UINT8 | TRACKER_VALUE, &masterConfig.max_speed_filter, .config.minmax = { 0, 255 } }
+ { "max_speed_filter", VAR_UINT8 | TRACKER_VALUE, &masterConfig.max_speed_filter, .config.minmax = { 0, 255 } },
+ { "telemetry_altitude_priority", VAR_UINT8 | TRACKER_VALUE | MODE_LOOKUP, &masterConfig.altitude_priority, .config.lookup = {TABLE_ALTITUDE_PRIORITY} }
};
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@@ -2530,7 +2537,7 @@ static void cliSetPanServoSpeed()
{
masterConfig.pan0_calibrated=0;
pwmPan0=masterConfig.pan0;
- pwmWritePanServo(masterConfig.pan0);
+ pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
cliPrint("\r\nWhen pan servo stops moving then you must set parameter pan0_calibrated to 1.\r\n");
}
static void cliSetOffset()
@@ -2571,7 +2578,7 @@ static void cliDumpTracker() {
printSectionBreak();
- cliPrint("\r\n\r\n# feature\r\n");
+ cliPrint("\r\n\r\n# parameters\r\n");
dumpValues(TRACKER_VALUE);
}
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index d64bd70..74fb4ba 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -705,11 +705,11 @@ void mspInit(serialConfig_t *serialConfig)
}
#endif
-#ifdef BLACKBOX
+/*#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)){
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
}
-#endif
+#endif*/
/*if (feature(FEATURE_FAILSAFE)){
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
diff --git a/src/main/main.c b/src/main/main.c
index 5baa5d1..fb96c58 100755
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -474,7 +474,7 @@ void init(void)
if (masterConfig.mixerMode == MIXER_GIMBAL) {
//accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
- //gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+ gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
//baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
diff --git a/src/main/mw.c b/src/main/mw.c
index cec17e2..7313ec6 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -305,11 +305,11 @@ void mwDisarm(void)
if (ARMING_FLAG(ARMED)) {
DISABLE_ARMING_FLAG(ARMED);
-#ifdef BLACKBOX
+/*#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
finishBlackbox();
}
-#endif
+#endif*/
beeper(BEEPER_DISARMING); // emit disarm tone
}
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 78ce6c3..d653a01 100755
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -45,10 +45,13 @@
#include "tracker/config.h"
#include "io/display.h"
+#include "tracker/servos.h"
+
mag_t mag; // mag access functions
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
+extern uint8_t pwmPanPin;
extern uint16_t pwmPan0;
extern uint16_t pwmPanCalibrationPulse;
@@ -97,7 +100,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
displayShowFixedPage(PAGE_CALIBRATING_MAG);
displayResetPageCycling();
displayDisablePageCycling();
- pwmWriteServo(panServo, pwmPanCalibrationPulse);
+ pwmWriteServo(pwmPanPin, pwmPanCalibrationPulse);
}
if (magInit) { // we apply offset only once mag calibration is done
@@ -120,7 +123,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
for (axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}
- pwmWriteServo(panServo, pwmPan0);
+ pwmWriteServo(pwmPanPin, pwmPan0);
DISABLE_PROTOCOL(TP_CALIBRATING_MAG);
if(cliMode && feature(FEATURE_DISPLAY)) {
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index e3392b6..1d7223a 100755
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -161,7 +161,7 @@ static void ltm_gframe(void)
ltm_initialise_packet('G');
ltm_serialise_32(targetPosition.lat * 10);
ltm_serialise_32(targetPosition.lon * 10);
- ltm_serialise_8((uint8_t)(telemetry_speed * 100)); // / 100));
+ ltm_serialise_8((uint8_t)(telemetry_speed)); // / 100));
//printf("speed %d course %d\n",telemetry_speed,telemetry_course);
ltm_serialise_32(targetPosition.alt*100);
ltm_serialise_8((telemetry_sats << 2) | gps_fix_type);
diff --git a/src/main/tracker/config.h b/src/main/tracker/config.h
index a6beaa7..82a8603 100755
--- a/src/main/tracker/config.h
+++ b/src/main/tracker/config.h
@@ -19,7 +19,4 @@
#ifndef CONFIG_H
#define CONFIG_H
-#define panServo 0
-#define TILT_SERVO 1
-
#endif
diff --git a/src/main/tracker/frskyx.c b/src/main/tracker/frskyx.c
index 5845efc..77d4369 100755
--- a/src/main/tracker/frskyx.c
+++ b/src/main/tracker/frskyx.c
@@ -140,10 +140,10 @@ void processHubPacket(uint8_t id, uint16_t value)
NS = value;
break;
case TEMP2:
- if(telemetry_provider==1){
+ if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
sats = value / 10;
fix = value % 10;
- } else if (telemetry_provider==2){
+ } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
sats = value % 100;
} else
sats = value;
@@ -185,6 +185,8 @@ void processSportPacket(uint8_t *packet)
return;
}
+ if(appId == GPS_STATUS) telemetry_provider = TELEMETRY_PROVIDER_APM10;
+
switch (prim)
{
case DATA_FRAME:
@@ -195,10 +197,10 @@ void processSportPacket(uint8_t *packet)
processHubPacket(id, value);
}
else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
- if(telemetry_provider==1){
+ if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
sats = SPORT_DATA_S32(packet) / 10;
//fix = SPORT_DATA_S32(packet) % 10;
- } else if (telemetry_provider==2){
+ } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
sats = SPORT_DATA_S32(packet) % 100;
} else {
//we assume that other systems just send the sats over temp2 and fix over temp1
@@ -252,17 +254,14 @@ void processSportPacket(uint8_t *packet)
}
}
- else if(appId == GPS_STATUS && telemetry_provider==3){
+ else if(appId == GPS_STATUS && telemetry_provider == TELEMETRY_PROVIDER_APM10){
sats = (uint8_t) (SPORT_DATA_U32(packet) & bitmask(4,0));
- }
- else if(appId == AIRCRAFT_HOME && telemetry_provider==3){
- uint16_t msl_dc, msl_x, msl_sgn;
- msl_dc = (SPORT_DATA_U32(packet) & bitmask(7,24)) >> 24;
- msl_x = (SPORT_DATA_U32(packet) & bitmask(2,22)) >> 22;
- msl_sgn = (SPORT_DATA_U32(packet) & bitmask(1,31)) >> 31;
- //homeAlt = bit32.extract(VALUE,14,10) * (10^bit32.extract(VALUE,12,2)) * 0.1 * (bit32.extract(VALUE,24,1) == 1 and -1 or 1) --m
- alt = ((int16_t) (msl_dc * msl_x * 0.1 * msl_sgn == 1?-1:1));
- telemetry_alt = alt;
+ //telemetry.gpsAlt = bit32.extract(VALUE,24,7) * (10^bit32.extract(VALUE,22,2)) * (bit32.extract(VALUE,31,1) == 1 and -1 or 1) -- dm
+ uint16_t msl_dc, msl_x, msl_sgn;
+ msl_dc = (SPORT_DATA_U32(packet) & bitmask(7,24)) >> 24;
+ msl_x = (SPORT_DATA_U32(packet) & bitmask(2,22)) >> 22;
+ msl_sgn = (SPORT_DATA_U32(packet) & bitmask(1,31)) >> 31;
+ telemetry_alt = (int16_t) (msl_dc * pow(10,msl_x) * 0.1 * (msl_sgn == 1?-1:1));
gotAlt = true;
}
break;
diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c
index 0ca9c43..c81ac2a 100755
--- a/src/main/tracker/main.c
+++ b/src/main/tracker/main.c
@@ -154,6 +154,8 @@ void telemetryPortInit(void);
void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep);
void calcTelemetryFrequency(void);
uint8_t filterTiltAngle(uint8_t target);
+void servosInit(void);
+
//EASING
int16_t _lastTilt;
int16_t tilt;
@@ -354,6 +356,7 @@ void tracker_setup(void)
epsVectorsInit(&targetLast,&targetCurrent,&targetEstimated,masterConfig.eps_interpolation,masterConfig.eps_interpolation_points);
+ targetPosition.home_alt = -32768;
}
void telemetryPortInit(void){
@@ -387,8 +390,6 @@ void trackingInit(void){
menuState = 0;
- targetPosition.home_alt = -32768;
-
telemetry_frequency = 0;
telemetry_millis = millis();
}
@@ -515,7 +516,7 @@ void calcTilt(void) {
}
else {
pwmTilt = (uint16_t) map(tiltTarget,0,90,masterConfig.tilt0, masterConfig.tilt90);
- pwmWriteTiltServo(pwmTilt);
+ pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
}
}
@@ -603,13 +604,13 @@ void servo_tilt_update(){
easingout = _lastTilt - easeTilt(_tilt_pos, 0, _lastTilt - _servo_tilt_must_move, masterConfig.easing_steps);
pwmTilt=(int16_t)map(easingout,0,90,masterConfig.tilt0,masterConfig.tilt90);
if(_lastPwmTilt != pwmTilt)
- pwmWriteTiltServo(pwmTilt);
+ pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
_tilt_pos++;
}
else {
if(_tilt_pos == masterConfig.easing_steps){
pwmTilt = (uint16_t) map(_servo_tilt_must_move,0,90, masterConfig.tilt0, masterConfig.tilt90);
- pwmWriteTiltServo(pwmTilt);
+ pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
_lastTilt = _servo_tilt_must_move;
_tilt_pos=0;
_servo_tilt_has_arrived = true;
@@ -778,7 +779,7 @@ void updateServoTest(void){
else
{
tilt = map(SERVOTEST_TILT, 0, 90, masterConfig.tilt0, masterConfig.tilt90);
- pwmWriteTiltServo(tilt);
+ pwmWriteServo(masterConfig.tilt_pin,tilt);
}
DISABLE_SERVO(SERVOTILT_MOVE);
}
@@ -967,6 +968,7 @@ void updateMenuButton(void){
//start calibration routine if button pressed > 4s and released
//ENABLE_STATE(CALIBRATE_MAG);
calib_timer = 0;
+ home_timer = 0;
enterMenuMode();
}
}
@@ -1100,14 +1102,13 @@ bool couldTelemetrySetHome(void){
void updateMFD(void){
if(PROTOCOL(TP_MFD)){
- if (settingHome) {
- homeSet = true;
- settingHome = 0;
- }
+
+ homeSet = true;
+ settingHome = 0;
if (mfdTestMode || (homeSet && gotFix)) {
targetPosition.distance = getDistance();
- targetPosition.alt = getTargetAlt(targetPosition.home_alt);
+ targetPosition.alt = getTargetAlt(0);
targetPosition.heading = getAzimuth() * 10;
gotFix = false;
}
@@ -1115,10 +1116,15 @@ void updateMFD(void){
if ((mfdTestMode || homeSet) && gotNewHeading) {
getError();
calculatePID();
- pwmWritePanServo(pwmPan);
+ pwmWriteServo(masterConfig.pan_pin,pwmPan);
calcTilt();
gotNewHeading = false;
}
+
+ if(homeSet && lostTelemetry == true && !cliMode){
+ pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
+ return;
+ }
}
}
@@ -1134,7 +1140,7 @@ void updateTracking(void){
if(trackingStarted) {
if(lostTelemetry == true && !cliMode){
- pwmWritePanServo(masterConfig.pan0);
+ pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
return;
}
if(!PROTOCOL(TP_SERVOTEST))
@@ -1143,7 +1149,7 @@ void updateTracking(void){
if (gotNewHeading) {
getError();
calculatePID();
- pwmWritePanServo(pwmPan);
+ pwmWriteServo(masterConfig.pan_pin,pwmPan);
gotNewHeading = false;
}
@@ -1153,7 +1159,7 @@ void updateTracking(void){
} else {
OFFSET_TRIM_STATE = TRIM_STATE_DISABLED;
if(homeSet && !cliMode )
- pwmWritePanServo(masterConfig.pan0);
+ pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
}
}
}
@@ -1388,7 +1394,7 @@ void updateCalibratePan()
DISABLE_STATE(CALIBRATE_PAN);
ENABLE_PROTOCOL(TP_CALIBRATING_PAN0);
pwmPan = masterConfig.pan_calibration_pulse;
- pwmWriteServo(panServo, pwmPan);
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);
masterConfig.pan0_calibrated = 0;
minPwmPan = 1500;
maxPwmPan = 1500;
@@ -1419,7 +1425,7 @@ void updateCalibratePan()
if(pwmPan < minPwmPan - 100)
pwmPan = maxPwmPan + 100;
}
- pwmWriteServo(panServo, pwmPan);
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);
} else {
// SERVO SEEMS TO BE STOPPED
if(pwmPanState == FINDING_OUT_MIN_PWMPAN0){
@@ -1452,7 +1458,7 @@ void updateCalibratePan()
pwmPanState = FINDING_OUT_MAX_PWMPAN0;
pwmPan = 1500 + (1500 - masterConfig.pan_calibration_pulse);
}
- pwmWriteServo(panServo, pwmPan);
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);
}
else {
// SERVO IS STOPED
@@ -1460,7 +1466,7 @@ void updateCalibratePan()
//targetPosition.heading = trackerPosition.heading;
pwmPanState = FINDING_OUT_MAX_PWMPAN0;
pwmPan = 1500 + (1500 - masterConfig.pan_calibration_pulse);
- pwmWriteServo(panServo, pwmPan);
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);
} else if(pwmPanState == MAX_PWMPAN0_FOUND) {
// CALIBRATION FIHISHED WITH SUCCESS
pwmPanState = PWMPAN0_CALCULATED_WITH_SUCCESS;
@@ -1477,7 +1483,7 @@ void updateCalibratePan()
// ACTIVATE MAX PWMPAN CALCULATION
/*ENABLE_PROTOCOL(TP_CALIBRATING_MAXPAN);
pwmPan = masterConfig.pan0 - 600;
- pwmWriteServo(panServo, pwmPan);*/
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);*/
trackerPosition.heading = getHeading();
targetPosition.heading = trackerPosition.heading;
}
@@ -1507,11 +1513,11 @@ void updateCalibratePan()
if(pwmPan > masterConfig.pan0 + 600) {
//Calcular m�ximo y finalizar
masterConfig.max_pid_gain = maxPwmPan;
- pwmWriteServo(panServo, masterConfig.pan0);
+ pwmWriteServo(masterConfig.pan_pin, masterConfig.pan0);
DISABLE_PROTOCOL(TP_CALIBRATING_MAXPAN);
saveConfigAndNotify();
} else {
- pwmWriteServo(panServo, pwmPan);
+ pwmWriteServo(masterConfig.pan_pin, pwmPan);
trackerPosition.heading = getHeading();
targetPosition.heading = trackerPosition.heading;
servoPanTimer = millis();
@@ -1637,3 +1643,8 @@ void calcTelemetryFrequency(void){
}
}
+void servosInit(void)
+{
+ pwmWriteServo(masterConfig.pan_pin, masterConfig.pan0);
+ pwmWriteServo(masterConfig.tilt_pin, masterConfig.tilt0);
+}
diff --git a/src/main/tracker/mavlink.c b/src/main/tracker/mavlink.c
index f5181cb..6c4b894 100755
--- a/src/main/tracker/mavlink.c
+++ b/src/main/tracker/mavlink.c
@@ -35,6 +35,7 @@ void mavlink_handleMessage(mavlink_message_t* msg) {
telemetry_sats = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
telemetry_alt = (int16_t)(mavlink_msg_gps_raw_int_get_alt(msg) / 1000);
telemetry_fixtype = mavlink_msg_gps_raw_int_get_fix_type(msg);
+ telemetry_speed = mavlink_msg_gps_raw_int_get_vel(msg) / 100;
gotAlt = true;
// fix_type: GPS lock 0-1=no fix, 2=2D, 3=3D
@@ -70,10 +71,9 @@ void mavlink_handleMessage(mavlink_message_t* msg) {
void mavlink_encodeTargetData(uint8_t c) {
mavlink_status_t status;
mavlink_message_t msg;
- if (mavlink_parse_char(0, c, &msg, &status)) {
+ if (mavlink_parse_char(0, c, &msg, &status))
mavlink_handleMessage(&msg);
- if (status.parse_error > 0)
- telemetry_failed_cs++;
- }
+ else
+ telemetry_failed_cs += status.packet_rx_drop_count;
}
diff --git a/src/main/tracker/servos.h b/src/main/tracker/servos.h
index 97f8fb1..517046c 100755
--- a/src/main/tracker/servos.h
+++ b/src/main/tracker/servos.h
@@ -20,27 +20,4 @@
#ifndef SERVOS_H
#define SERVOS_H
-#include "config.h"
-#include "drivers/pwm_output.h"
-
-void servosInit(void);
-void pwmWritePanServo(int16_t A);
-void pwmWriteTiltServo(int16_t A);
-
-void servosInit(void)
-{
- pwmWritePanServo(masterConfig.pan0);
- pwmWriteTiltServo(masterConfig.tilt0);
-}
-void pwmWritePanServo(int16_t A){
- pwmWriteServo(panServo, A);
-}
-
-void stopPanServo(void){
- pwmWritePanServo(masterConfig.pan0);
-}
-
-void pwmWriteTiltServo(int16_t A){
- pwmWriteServo(TILT_SERVO, A);
-}
#endif
diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h
index fa9e70a..cc709a0 100755
--- a/src/main/tracker/telemetry.h
+++ b/src/main/tracker/telemetry.h
@@ -16,6 +16,8 @@
* along with u360gts. If not, see .
*
*/
+#ifndef TELEMETRY_H
+#define TELEMETRY_H
#include
#include
@@ -52,6 +54,13 @@ extern int32_t telemetry_home_lat;
extern int32_t telemetry_home_lon;
extern int16_t telemetry_home_alt;
+enum telemetryProviders {
+ TELEMETRY_PROVIDER_NONE = 0,
+ TELEMETRY_PROVIDER_DIY_GPS,
+ TELEMETRY_PROVIDER_INAV,
+ TELEMETRY_PROVIDER_APM10
+};
+
int16_t getTargetAlt(int16_t home_alt);
void encodeTargetData(uint8_t c);
int32_t getTargetLat();
@@ -63,3 +72,4 @@ void enableProtocolDetection(void);
void disableProtocolDetection(void);
void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt);
+#endif
diff --git a/src/main/version.h b/src/main/version.h
index 7775862..353ad41 100755
--- a/src/main/version.h
+++ b/src/main/version.h
@@ -16,8 +16,8 @@
*/
#define FC_VERSION_MAJOR 10 // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
+#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_PATCH_LEVEL 3 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)