mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-14 20:10:15 +03:00
Bugfixes and prepare configurator integration
This commit is contained in:
Scavanger
parent
9330784729
commit
f330bdd621
27 changed files with 166 additions and 246 deletions
|
|
@ -50,7 +50,6 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
|
|||
OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
|
||||
OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
|
||||
OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
|
||||
OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -35,6 +35,10 @@
|
|||
|
||||
#include "fc/settings.h"
|
||||
|
||||
#ifdef SITL_BUILD
|
||||
#include <time.h>
|
||||
#endif
|
||||
|
||||
// For the "modulo 4" arithmetic to work, we need a leap base year
|
||||
#define REFERENCE_YEAR 2000
|
||||
// Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01
|
||||
|
|
@ -310,11 +314,16 @@ bool rtcHasTime(void)
|
|||
|
||||
bool rtcGet(rtcTime_t *t)
|
||||
{
|
||||
#ifdef SITL_BUILD
|
||||
*t = (rtcTime_t)(time(NULL) * 1000);
|
||||
return true;
|
||||
#else
|
||||
if (!rtcHasTime()) {
|
||||
return false;
|
||||
}
|
||||
*t = started + millis();
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool rtcSet(rtcTime_t *t)
|
||||
|
|
@ -323,6 +332,7 @@ bool rtcSet(rtcTime_t *t)
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool rtcGetDateTime(dateTime_t *dt)
|
||||
{
|
||||
rtcTime_t t;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ typedef uint32_t timeUs_t;
|
|||
#define USECS_PER_SEC (1000 * 1000)
|
||||
|
||||
#define HZ2US(hz) (1000000 / (hz))
|
||||
#define HZ2MS(hz) (1000 / (hz))
|
||||
#define US2S(us) ((us) * 1e-6f)
|
||||
#define US2MS(us) ((us) * 1e-3f)
|
||||
#define MS2US(ms) ((ms) * 1000)
|
||||
|
|
|
|||
|
|
@ -57,3 +57,7 @@ int config_streamer_flush(config_streamer_t *c);
|
|||
|
||||
int config_streamer_finish(config_streamer_t *c);
|
||||
int config_streamer_status(config_streamer_t *c);
|
||||
|
||||
#if defined(CONFIG_IN_FILE)
|
||||
bool configFileSetPath(char* path);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <dirent.h>
|
||||
#include "platform.h"
|
||||
#include "drivers/system.h"
|
||||
#include "config/config_streamer.h"
|
||||
|
|
@ -26,21 +27,32 @@
|
|||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
|
||||
|
||||
#define FLASH_PAGE_SIZE (0x400)
|
||||
|
||||
static FILE *eepromFd = NULL;
|
||||
|
||||
static bool streamerLocked = true;
|
||||
static char eepromPath[260] = EEPROM_FILENAME;
|
||||
|
||||
bool configFileSetPath(char* path)
|
||||
{
|
||||
if(!path || strlen(path) > 260) {
|
||||
return false;
|
||||
}
|
||||
|
||||
strcpy(eepromPath, path);
|
||||
return true;
|
||||
}
|
||||
|
||||
void config_streamer_impl_unlock(void)
|
||||
{
|
||||
if (eepromFd != NULL) {
|
||||
fprintf(stderr, "[EEPROM] Unable to load %s\n", EEPROM_FILENAME);
|
||||
fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
|
||||
return;
|
||||
}
|
||||
|
||||
// open or create
|
||||
eepromFd = fopen(EEPROM_FILENAME,"r+");
|
||||
eepromFd = fopen(eepromPath,"r+");
|
||||
if (eepromFd != NULL) {
|
||||
// obtain file size:
|
||||
fseek(eepromFd , 0 , SEEK_END);
|
||||
|
|
@ -49,16 +61,16 @@ void config_streamer_impl_unlock(void)
|
|||
|
||||
size_t n = fread(eepromData, 1, sizeof(eepromData), eepromFd);
|
||||
if (n == size) {
|
||||
printf("[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", EEPROM_FILENAME, size, sizeof(eepromData));
|
||||
fprintf(stderr,"[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", eepromPath, size, sizeof(eepromData));
|
||||
streamerLocked = false;
|
||||
} else {
|
||||
fprintf(stderr, "[EEPROM] Failed to load '%s'\n", EEPROM_FILENAME);
|
||||
fprintf(stderr, "[EEPROM] Failed to load '%s'\n", eepromPath);
|
||||
}
|
||||
} else {
|
||||
printf("[EEPROM] Created '%s', size = %ld\n", EEPROM_FILENAME, sizeof(eepromData));
|
||||
printf("[EEPROM] Created '%s', size = %ld\n", eepromPath, sizeof(eepromData));
|
||||
streamerLocked = false;
|
||||
if ((eepromFd = fopen(EEPROM_FILENAME, "w+")) == NULL) {
|
||||
fprintf(stderr, "[EEPROM] Failed to create '%s'\n", EEPROM_FILENAME);
|
||||
if ((eepromFd = fopen(eepromPath, "w+")) == NULL) {
|
||||
fprintf(stderr, "[EEPROM] Failed to create '%s'\n", eepromPath);
|
||||
streamerLocked = true;
|
||||
}
|
||||
if (fwrite(eepromData, sizeof(eepromData), 1, eepromFd) != 1) {
|
||||
|
|
@ -66,8 +78,6 @@ void config_streamer_impl_unlock(void)
|
|||
streamerLocked = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void config_streamer_impl_lock(void)
|
||||
|
|
@ -78,7 +88,7 @@ void config_streamer_impl_lock(void)
|
|||
fwrite(eepromData, 1, sizeof(eepromData), eepromFd);
|
||||
fclose(eepromFd);
|
||||
eepromFd = NULL;
|
||||
printf("[EEPROM] Saved '%s'\n", EEPROM_FILENAME);
|
||||
fprintf(stderr, "[EEPROM] Saved '%s'\n", eepromPath);
|
||||
streamerLocked = false;
|
||||
} else {
|
||||
fprintf(stderr, "[EEPROM] Unlock error\n");
|
||||
|
|
@ -93,9 +103,9 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
|
|||
|
||||
if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) {
|
||||
*((uint32_t*)c->address) = *buffer;
|
||||
printf("[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
|
||||
fprintf(stderr, "[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
|
||||
} else {
|
||||
printf("[EEPROM] Program word %p out of range!\n", (void*)c->address);
|
||||
fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
|
||||
}
|
||||
|
||||
c->address += CONFIG_STREAMER_BUFFER_SIZE;
|
||||
|
|
|
|||
|
|
@ -39,9 +39,6 @@
|
|||
static pthread_mutex_t gyroMutex;
|
||||
static pthread_mutex_t accMutex;
|
||||
|
||||
#define LOCK(mutex) (pthread_mutex_lock(mutex))
|
||||
#define UNLOCK(mutex) (pthread_mutex_unlock(mutex))
|
||||
|
||||
#define GYROLOCK (pthread_mutex_lock(&gyroMutex))
|
||||
#define GYROUNLOCK (pthread_mutex_unlock(&gyroMutex))
|
||||
#define ACCLOCK (pthread_mutex_lock(&accMutex))
|
||||
|
|
@ -65,8 +62,6 @@ static void fakeGyroInit(gyroDev_t *gyro)
|
|||
#if defined(SITL_BUILD)
|
||||
pthread_mutex_init(&gyroMutex, NULL);
|
||||
#endif
|
||||
|
||||
//ENABLE_STATE(ACCELEROMETER_CALIBRATED);
|
||||
}
|
||||
|
||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
||||
|
|
|
|||
|
|
@ -3417,7 +3417,11 @@ static void cliStatus(char *cmdline)
|
|||
cliPrint("OSD: ");
|
||||
#if defined(USE_OSD)
|
||||
displayPort_t *osdDisplayPort = osdGetDisplayPort();
|
||||
if (osdDisplayPort) {
|
||||
cliPrintf("%s [%u x %u]", osdDisplayPort->displayPortType, osdDisplayPort->cols, osdDisplayPort->rows);
|
||||
} else {
|
||||
cliPrintf("no OSD detected");
|
||||
}
|
||||
#else
|
||||
cliPrint("not used");
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -867,6 +867,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
if (isRXDataNew) {
|
||||
updateWaypointsAndNavigationMode();
|
||||
}
|
||||
|
||||
isRXDataNew = false;
|
||||
|
||||
updatePositionEstimator();
|
||||
|
|
@ -929,9 +930,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
writeMotors();
|
||||
}
|
||||
#endif
|
||||
|
||||
// Check if landed, FW and MR
|
||||
if (STATE(ALTITUDE_CONTROL)) {
|
||||
updateLandingStatus(US2MS(currentTimeUs));
|
||||
updateLandingStatus();
|
||||
}
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
|
|
|
|||
|
|
@ -46,11 +46,7 @@
|
|||
#include "drivers/compass/compass_msp.h"
|
||||
#include "drivers/barometer/barometer_msp.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_msp.h"
|
||||
#include "drivers/accgyro/accgyro_fake.h"
|
||||
#include "drivers/barometer/barometer_fake.h"
|
||||
#include "drivers/compass/compass_fake.h"
|
||||
#include "sensors/battery_sensor_fake.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_fake.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/display.h"
|
||||
#include "drivers/flash.h"
|
||||
|
|
|
|||
|
|
@ -1393,22 +1393,22 @@ groups:
|
|||
type: imuConfig_t
|
||||
headers: ["flight/imu.h"]
|
||||
members:
|
||||
- name: ahrs_dcm_kp
|
||||
- name: imu_dcm_kp
|
||||
description: "Inertial Measurement Unit KP Gain for accelerometer measurements"
|
||||
default_value: 2000
|
||||
field: dcm_kp_acc
|
||||
max: UINT16_MAX
|
||||
- name: ahrs_dcm_ki
|
||||
- name: imu_dcm_ki
|
||||
description: "Inertial Measurement Unit KI Gain for accelerometer measurements"
|
||||
default_value: 50
|
||||
field: dcm_ki_acc
|
||||
max: UINT16_MAX
|
||||
- name: ahrs_dcm_kp_mag
|
||||
- name: imu_dcm_kp_mag
|
||||
description: "Inertial Measurement Unit KP Gain for compass measurements"
|
||||
default_value: 2000
|
||||
field: dcm_kp_mag
|
||||
max: UINT16_MAX
|
||||
- name: ahrs_dcm_ki_mag
|
||||
- name: imu_dcm_ki_mag
|
||||
description: "Inertial Measurement Unit KI Gain for compass measurements"
|
||||
default_value: 50
|
||||
field: dcm_ki_mag
|
||||
|
|
@ -1418,24 +1418,24 @@ groups:
|
|||
default_value: 25
|
||||
min: 0
|
||||
max: 180
|
||||
- name: ahrs_acc_ignore_rate
|
||||
- name: imu_acc_ignore_rate
|
||||
description: "Total gyro rotation rate threshold [deg/s] before scaling to consider accelerometer trustworthy"
|
||||
default_value: 15
|
||||
field: acc_ignore_rate
|
||||
min: 0
|
||||
max: 30
|
||||
- name: ahrs_acc_ignore_slope
|
||||
- name: imu_acc_ignore_slope
|
||||
description: "Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)"
|
||||
default_value: 5
|
||||
field: acc_ignore_slope
|
||||
min: 0
|
||||
max: 10
|
||||
- name: ahrs_gps_yaw_windcomp
|
||||
- name: imu_gps_yaw_windcomp
|
||||
description: "Wind compensation in heading estimation from gps groundcourse(fixed wing only)"
|
||||
default_value: ON
|
||||
field: gps_yaw_windcomp
|
||||
type: bool
|
||||
- name: ahrs_inertia_comp_method
|
||||
- name: imu_inertia_comp_method
|
||||
description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop"
|
||||
default_value: VELNED
|
||||
field: inertia_comp_method
|
||||
|
|
@ -2563,7 +2563,7 @@ groups:
|
|||
max: 45
|
||||
- name: nav_auto_disarm_delay
|
||||
description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)"
|
||||
default_value: 1000
|
||||
default_value: 2000
|
||||
field: general.auto_disarm_delay
|
||||
min: 100
|
||||
max: 10000
|
||||
|
|
|
|||
|
|
@ -346,11 +346,6 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
|
|||
}
|
||||
}
|
||||
|
||||
// Inhibit Failsafe if emergency landing triggered manually
|
||||
if (posControl.flags.manualEmergLandActive) {
|
||||
return FAILSAFE_PROCEDURE_NONE;
|
||||
}
|
||||
|
||||
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
|
||||
// GPS must also be working, and home position set
|
||||
if (failsafeConfig()->failsafe_min_distance > 0 &&
|
||||
|
|
|
|||
|
|
@ -116,15 +116,15 @@ FASTRAM bool imuUpdated = false;
|
|||
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
|
||||
.dcm_kp_acc = SETTING_AHRS_DCM_KP_DEFAULT, // 0.20 * 10000
|
||||
.dcm_ki_acc = SETTING_AHRS_DCM_KI_DEFAULT, // 0.005 * 10000
|
||||
.dcm_kp_mag = SETTING_AHRS_DCM_KP_MAG_DEFAULT, // 0.20 * 10000
|
||||
.dcm_ki_mag = SETTING_AHRS_DCM_KI_MAG_DEFAULT, // 0.005 * 10000
|
||||
.dcm_kp_acc = SETTING_IMU_DCM_KP_DEFAULT, // 0.20 * 10000
|
||||
.dcm_ki_acc = SETTING_IMU_DCM_KI_DEFAULT, // 0.005 * 10000
|
||||
.dcm_kp_mag = SETTING_IMU_DCM_KP_MAG_DEFAULT, // 0.20 * 10000
|
||||
.dcm_ki_mag = SETTING_IMU_DCM_KI_MAG_DEFAULT, // 0.005 * 10000
|
||||
.small_angle = SETTING_SMALL_ANGLE_DEFAULT,
|
||||
.acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
|
||||
.acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
|
||||
.gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
|
||||
.inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
|
||||
.acc_ignore_rate = SETTING_IMU_ACC_IGNORE_RATE_DEFAULT,
|
||||
.acc_ignore_slope = SETTING_IMU_ACC_IGNORE_SLOPE_DEFAULT,
|
||||
.gps_yaw_windcomp = 1,
|
||||
.inertia_comp_method = COMPMETHOD_VELNED
|
||||
);
|
||||
|
||||
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
||||
|
|
|
|||
|
|
@ -228,6 +228,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
|
|||
case SYM_WIND_VERTICAL:
|
||||
return BF_SYM_WIND_VERTICAL;
|
||||
|
||||
case SYM_3D_KMH:
|
||||
return BF_SYM_3D_KMH;
|
||||
|
||||
case SYM_3D_MPH:
|
||||
return BF_SYM_3D_MPH;
|
||||
|
||||
case SYM_3D_KT:
|
||||
return BF_SYM_3D_KT;
|
||||
|
||||
|
|
@ -236,12 +242,6 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
|
|||
return BF_SYM_AIR;
|
||||
*/
|
||||
|
||||
case SYM_3D_KMH:
|
||||
return BF_SYM_KPH;
|
||||
|
||||
case SYM_3D_MPH:
|
||||
return BF_SYM_MPH;
|
||||
|
||||
case SYM_RPM:
|
||||
return BF_SYM_RPM;
|
||||
|
||||
|
|
|
|||
|
|
@ -1774,13 +1774,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
// TODO:
|
||||
UNUSED(previousState);
|
||||
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
resetPositionController();
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
|
||||
// Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
|
||||
// Make sure terrain following is not enabled
|
||||
resetAltitudeController(false);
|
||||
|
|
@ -1792,14 +1788,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PR
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
// Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
|
||||
if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
}
|
||||
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
@ -2813,18 +2801,12 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
|
|||
/*-----------------------------------------------------------
|
||||
* NAV land detector
|
||||
*-----------------------------------------------------------*/
|
||||
void updateLandingStatus(timeMs_t currentTimeMs)
|
||||
void updateLandingStatus(void)
|
||||
{
|
||||
if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
|
||||
return; // no point using this with a fixed wing if not set to disarm
|
||||
}
|
||||
|
||||
static timeMs_t lastUpdateTimeMs = 0;
|
||||
if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz
|
||||
return;
|
||||
}
|
||||
lastUpdateTimeMs = currentTimeMs;
|
||||
|
||||
static bool landingDetectorIsActive;
|
||||
|
||||
DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
|
||||
|
|
@ -2849,7 +2831,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
|
|||
if (navConfig()->general.flags.disarm_on_landing) {
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
disarm(DISARM_LANDING);
|
||||
} else if (!navigationInAutomaticThrottleMode()) {
|
||||
} else if (!navigationIsFlyingAutonomousMode()) {
|
||||
// for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
|
||||
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
|
||||
}
|
||||
|
|
@ -3433,10 +3415,7 @@ bool isNavHoldPositionActive(void)
|
|||
return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
|
||||
}
|
||||
// RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
|
||||
// Also hold position during emergency landing if position valid
|
||||
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
|
||||
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
|
||||
navigationIsExecutingAnEmergencyLanding();
|
||||
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE);
|
||||
}
|
||||
|
||||
float getActiveWaypointSpeed(void)
|
||||
|
|
@ -3535,7 +3514,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
// If we are disarmed, abort forced RTH or Emergency Landing
|
||||
posControl.flags.forcedRTHActivated = false;
|
||||
posControl.flags.forcedEmergLandingActivated = false;
|
||||
posControl.flags.manualEmergLandActive = false;
|
||||
// ensure WP missions always restart from first waypoint after disarm
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
// Reset RTH trackback
|
||||
|
|
@ -3612,41 +3590,6 @@ static bool isWaypointMissionValid(void)
|
|||
return posControl.waypointListValid && (posControl.waypointCount > 0);
|
||||
}
|
||||
|
||||
static void checkManualEmergencyLandingControl(void)
|
||||
{
|
||||
static timeMs_t timeout = 0;
|
||||
static int8_t counter = 0;
|
||||
static bool toggle;
|
||||
timeMs_t currentTimeMs = millis();
|
||||
|
||||
if (timeout && currentTimeMs > timeout) {
|
||||
timeout += 1000;
|
||||
counter -= counter ? 1 : 0;
|
||||
if (!counter) {
|
||||
timeout = 0;
|
||||
}
|
||||
}
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
|
||||
if (!timeout && toggle) {
|
||||
timeout = currentTimeMs + 4000;
|
||||
}
|
||||
counter += toggle;
|
||||
toggle = false;
|
||||
} else {
|
||||
toggle = true;
|
||||
}
|
||||
|
||||
// Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
|
||||
if (counter >= 5) {
|
||||
counter = 0;
|
||||
posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
|
||||
|
||||
if (!posControl.flags.manualEmergLandActive) {
|
||||
navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||
{
|
||||
static bool canActivateWaypoint = false;
|
||||
|
|
@ -3672,12 +3615,8 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
posControl.flags.rthTrackbackActive = isExecutingRTH;
|
||||
}
|
||||
|
||||
/* Emergency landing controlled manually by rapid switching of Poshold mode.
|
||||
* Landing toggled ON or OFF for each Poshold activation sequence */
|
||||
checkManualEmergencyLandingControl();
|
||||
|
||||
/* Emergency landing triggered by failsafe Landing or manually initiated */
|
||||
if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
|
||||
/* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */
|
||||
if (posControl.flags.forcedEmergLandingActivated) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -606,7 +606,7 @@ const char * fixedWingLaunchStateMessage(void);
|
|||
|
||||
float calculateAverageSpeed(void);
|
||||
|
||||
void updateLandingStatus(timeMs_t currentTimeMs);
|
||||
void updateLandingStatus(void);
|
||||
|
||||
const navigationPIDControllers_t* getNavigationPIDControllers(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -711,7 +711,7 @@ bool isFixedWingLandingDetected(void)
|
|||
static int16_t fwLandSetPitchDatum;
|
||||
const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
|
||||
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
timeMs_t currentTimeMs = millis();
|
||||
|
||||
// Check horizontal and vertical velocities are low (cm/s)
|
||||
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
|
||||
|
|
@ -736,12 +736,9 @@ bool isFixedWingLandingDetected(void)
|
|||
DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
|
||||
DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
|
||||
if (isRollAxisStatic && isPitchAxisStatic) {
|
||||
/* Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
|
||||
* Conditions need to be held for fixed safety time + optional extra delay.
|
||||
* Fixed time increased if velocities invalid to provide extra safety margin against false triggers */
|
||||
const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE || posControl.flags.estVelStatus == EST_NONE ? 5000 : 1000;
|
||||
timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
|
||||
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay;
|
||||
// Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
|
||||
timeMs_t safetyTimeDelay = 2000 + navConfig()->general.auto_disarm_delay;
|
||||
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
|
||||
} else {
|
||||
fixAxisCheck = false;
|
||||
}
|
||||
|
|
@ -755,6 +752,8 @@ bool isFixedWingLandingDetected(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
|
||||
{
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
|
||||
if (posControl.flags.estAltStatus >= EST_USABLE) {
|
||||
|
|
@ -766,18 +765,6 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
} else {
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
}
|
||||
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible
|
||||
applyFixedWingPositionController(currentTimeUs);
|
||||
int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
|
||||
-DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
|
||||
DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
rcCommand[YAW] = 0;
|
||||
} else {
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -723,7 +723,7 @@ bool isMulticopterFlying(void)
|
|||
bool isMulticopterLandingDetected(void)
|
||||
{
|
||||
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
||||
static timeMs_t landingDetectorStartedAt;
|
||||
static timeUs_t landingDetectorStartedAt;
|
||||
|
||||
/* Basic condition to start looking for landing
|
||||
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
|
||||
|
|
@ -747,7 +747,7 @@ bool isMulticopterLandingDetected(void)
|
|||
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
|
||||
|
||||
bool possibleLandingDetected = false;
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
|
||||
// We have likely landed if throttle is 40 units below average descend throttle
|
||||
|
|
@ -761,13 +761,13 @@ bool isMulticopterLandingDetected(void)
|
|||
|
||||
if (!landingDetectorStartedAt) {
|
||||
landingThrSum = landingThrSamples = 0;
|
||||
landingDetectorStartedAt = currentTimeMs;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
if (!landingThrSamples) {
|
||||
if (currentTimeMs - landingDetectorStartedAt < S2MS(MC_LAND_THR_STABILISE_DELAY)) { // Wait for 1 second so throttle has stabilized.
|
||||
if (currentTimeUs - landingDetectorStartedAt < (USECS_PER_SEC * MC_LAND_THR_STABILISE_DELAY)) { // Wait for 1 second so throttle has stabilized.
|
||||
return false;
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeMs;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
}
|
||||
landingThrSamples += 1;
|
||||
|
|
@ -783,7 +783,7 @@ bool isMulticopterLandingDetected(void)
|
|||
if (landingDetectorStartedAt) {
|
||||
possibleLandingDetected = velCondition && gyroCondition;
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeMs;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -798,13 +798,10 @@ bool isMulticopterLandingDetected(void)
|
|||
DEBUG_SET(DEBUG_LANDING, 5, possibleLandingDetected);
|
||||
|
||||
if (possibleLandingDetected) {
|
||||
/* Conditions need to be held for fixed safety time + optional extra delay.
|
||||
* Fixed time increased if Z velocity invalid to provide extra safety margin against false triggers */
|
||||
const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE ? 5000 : 1000;
|
||||
timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
|
||||
return currentTimeMs - landingDetectorStartedAt > safetyTimeDelay;
|
||||
timeUs_t safetyTimeDelay = MS2US(2000 + navConfig()->general.auto_disarm_delay); // check conditions stable for 2s + optional extra delay
|
||||
return (currentTimeUs - landingDetectorStartedAt > safetyTimeDelay);
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeMs;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -817,6 +814,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
static timeUs_t previousTimePositionUpdate = 0;
|
||||
|
||||
/* Attempt to stabilise */
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
|
||||
if ((posControl.flags.estAltStatus < EST_USABLE)) {
|
||||
|
|
@ -853,14 +852,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
|
||||
// Update throttle controller
|
||||
rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
|
||||
|
||||
// Hold position if possible
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
applyMulticopterPositionController(currentTimeUs);
|
||||
} else {
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -241,7 +241,7 @@ void onNewGPSData(void)
|
|||
|
||||
/* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
|
||||
float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
|
||||
if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
|
||||
if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
|
||||
posEstimator.gps.vel.x = gpsSol.velNED[X];
|
||||
posEstimator.gps.vel.y = gpsSol.velNED[Y];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@
|
|||
#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s
|
||||
#define MC_LAND_CHECK_VEL_Z_MOVING 100.0f // cm/s
|
||||
#define MC_LAND_THR_STABILISE_DELAY 1 // seconds
|
||||
#define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us)
|
||||
#define MC_LAND_DESCEND_THROTTLE 40 // uS
|
||||
#define MC_LAND_SAFE_SURFACE 5.0f // cm
|
||||
|
||||
#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points
|
||||
|
|
@ -104,13 +104,14 @@ typedef struct navigationFlags_s {
|
|||
bool forcedRTHActivated;
|
||||
bool forcedEmergLandingActivated;
|
||||
|
||||
bool wpMissionPlannerActive; // Activation status of WP mission planner
|
||||
|
||||
/* Landing detector */
|
||||
bool resetLandingDetector;
|
||||
|
||||
bool wpMissionPlannerActive; // Activation status of WP mission planner
|
||||
bool rthTrackbackActive; // Activation status of RTH trackback
|
||||
|
||||
bool wpTurnSmoothingActive; // Activation status WP turn smoothing
|
||||
bool manualEmergLandActive; // Activation status of manual emergency landing
|
||||
} navigationFlags_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
|
|||
|
|
@ -811,16 +811,12 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
|
|||
return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION:
|
||||
return (bool) FLIGHT_MODE(NAV_WP_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
|
||||
return ((bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !(bool)FLIGHT_MODE(NAV_ALTHOLD_MODE));
|
||||
return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
|
||||
return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
|
||||
return (bool)(FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
|
||||
|
|
@ -839,11 +835,6 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
|
|||
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
|
||||
return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) ||
|
||||
(bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
|
||||
return IS_RC_MODE_ACTIVE(BOXUSER1);
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -152,8 +152,6 @@ typedef enum {
|
|||
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD, // 11
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3, // 12
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15
|
||||
} logicFlightModeOperands_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -520,11 +520,6 @@ void accUpdate(void)
|
|||
|
||||
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
|
||||
performAcclerationCalibration();
|
||||
|
||||
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
|
||||
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
|
||||
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
|
||||
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
|
||||
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S8 DMA1_S6_CH3
|
||||
|
||||
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B
|
||||
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED, 0, 0), // WS2812B DMA2_S6_CH0
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
|
|
|
|||
|
|
@ -38,10 +38,6 @@
|
|||
#define BMI270_CS_PIN PC13
|
||||
#define IMU_BMI270_ALIGN CW270_DEG_FLIP
|
||||
|
||||
#define USE_IMU_ICM42605
|
||||
#define ICM42605_SPI_BUS BUS_SPI2
|
||||
#define ICM42605_CS_PIN PC13
|
||||
#define IMU_ICM42605_ALIGN CW180_DEG_FLIP
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_BUS BUS_SPI2
|
||||
|
|
|
|||
|
|
@ -414,7 +414,7 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
|
|||
// Wait until the connection is established, the interface has been initialised
|
||||
// and the first valid packet has been received to avoid problems with the startup calibration.
|
||||
while (!isInitalised) {
|
||||
// ...
|
||||
delay(250);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -184,11 +184,11 @@ static void* listenWorker(void* arg)
|
|||
|
||||
recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
|
||||
if (recvLen < 0 && errno != EWOULDBLOCK) {
|
||||
break;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (strncmp((char*)buf, "RREF", 4) != 0) {
|
||||
break;
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int i = 5; i < recvLen; i += 8) {
|
||||
|
|
@ -276,7 +276,6 @@ static void* listenWorker(void* arg)
|
|||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (hpath < 0) {
|
||||
|
|
@ -329,6 +328,8 @@ static void* listenWorker(void* arg)
|
|||
|
||||
if (!initalized) {
|
||||
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
|
||||
// Aircraft can wobble on the runway and prevents calibration of the accelerometer
|
||||
ENABLE_STATE(ACCELEROMETER_CALIBRATED);
|
||||
initalized = true;
|
||||
}
|
||||
|
||||
|
|
@ -362,14 +363,21 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
|
|||
return false;
|
||||
}
|
||||
|
||||
if (bind(sockFd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
|
||||
if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bind(sockFd, (struct sockaddr *) &addr, sizeof(addr));
|
||||
|
||||
serverAddr.sin_family = AF_INET;
|
||||
serverAddr.sin_addr.s_addr = inet_addr(ip);
|
||||
serverAddr.sin_port = htons(port);
|
||||
|
||||
if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
while (!initalized) {
|
||||
registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
|
||||
registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
|
||||
registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
|
||||
|
|
@ -389,15 +397,8 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
|
|||
registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
|
||||
registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
|
||||
registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
|
||||
|
||||
if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
while(!initalized) {
|
||||
//
|
||||
delay(250);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
|
@ -44,6 +44,7 @@
|
|||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "config/config_streamer.h"
|
||||
|
||||
#include "target/SITL/sim/realFlight.h"
|
||||
#include "target/SITL/sim/xplane.h"
|
||||
|
|
@ -66,46 +67,46 @@ static int simPort = 0;
|
|||
|
||||
void systemInit(void) {
|
||||
|
||||
printf("INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
|
||||
fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
|
||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||
printf("[SYSTEM] Init...\n");
|
||||
fprintf(stderr, "[SYSTEM] Init...\n");
|
||||
|
||||
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
|
||||
printf("[SYSTEM] Unable to create mainLoop lock.\n");
|
||||
fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (sitlSim != SITL_SIM_NONE) {
|
||||
printf("[SIM] Waiting for connection...\n");
|
||||
fprintf(stderr, "[SIM] Waiting for connection...\n");
|
||||
}
|
||||
|
||||
switch (sitlSim) {
|
||||
case SITL_SIM_REALFLIGHT:
|
||||
if (mappingCount > RF_MAX_PWM_OUTS) {
|
||||
printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
|
||||
fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
|
||||
sitlSim = SITL_SIM_NONE;
|
||||
break;
|
||||
}
|
||||
if (simRealFlightInit(simIp, pwmMapping, mappingCount, useImu)) {
|
||||
printf("[SIM] Connection with RealFlight (%s) successfully established. \n", simIp);
|
||||
fprintf(stderr, "[SIM] Connection with RealFlight successfully established.\n");
|
||||
} else {
|
||||
printf("[SIM] Connection with RealFlight (%s) NOT established. \n", simIp);
|
||||
fprintf(stderr, "[SIM] Connection with RealFlight NOT established.\n");
|
||||
}
|
||||
break;
|
||||
case SITL_SIM_XPLANE:
|
||||
if (mappingCount > XP_MAX_PWM_OUTS) {
|
||||
printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
|
||||
fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
|
||||
sitlSim = SITL_SIM_NONE;
|
||||
break;
|
||||
}
|
||||
if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) {
|
||||
printf("[SIM] Connection with XPlane successfully established. \n");
|
||||
fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n");
|
||||
} else {
|
||||
printf("[SIM] Connection with XPLane NOT established. \n");
|
||||
fprintf(stderr, "[SIM] Connection with X-PLane NOT established.\n");
|
||||
}
|
||||
break;
|
||||
default:
|
||||
printf("[SIM] No interface specified. Configurator only.\n");
|
||||
fprintf(stderr, "[SIM] No interface specified. Configurator only.\n");
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -149,21 +150,22 @@ bool parseMapping(char* mapStr)
|
|||
|
||||
void printCmdLineOptions(void)
|
||||
{
|
||||
printf("Avaiable options:\n");
|
||||
printf("--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
|
||||
printf("--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.");
|
||||
printf("--simport=[port] Port oft the simulator host.");
|
||||
printf("--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).");
|
||||
printf("--chanmap=[mapstring] Channel mapping, Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
|
||||
printf(" The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER_OUT>,)... All numbers must have two digits\n");
|
||||
printf(" For example map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
|
||||
printf(" --chanmap=M01-01,S01-02,S02-03\n");
|
||||
fprintf(stderr, "Avaiable options:\n");
|
||||
fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
|
||||
fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
|
||||
fprintf(stderr, "--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
|
||||
fprintf(stderr, "--simport=[port] Port oft the simulator host.\n");
|
||||
fprintf(stderr, "--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
|
||||
fprintf(stderr, "--chanmap=[mapstring] Channel mapping, Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
|
||||
fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER_OUT>,)... All numbers must have two digits\n");
|
||||
fprintf(stderr, " For example map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
|
||||
fprintf(stderr, " --chanmap=M01-01,S01-02,S02-03\n");
|
||||
}
|
||||
|
||||
void parseArguments(int argc, char *argv[])
|
||||
{
|
||||
int c;
|
||||
while(1) {
|
||||
while(true) {
|
||||
static struct option longOpt[] = {
|
||||
{"sim", optional_argument, 0, 's'},
|
||||
{"useimu", optional_argument, 0, 'u'},
|
||||
|
|
@ -171,6 +173,7 @@ void parseArguments(int argc, char *argv[])
|
|||
{"simip", optional_argument, 0, 'i'},
|
||||
{"simport", optional_argument, 0, 'p'},
|
||||
{"help", optional_argument, 0, 'h'},
|
||||
{"path", optional_argument, 0, 'e'},
|
||||
{NULL, 0, NULL, 0}
|
||||
};
|
||||
|
||||
|
|
@ -185,13 +188,13 @@ void parseArguments(int argc, char *argv[])
|
|||
} else if (strcmp(optarg, "xp") == 0){
|
||||
sitlSim = SITL_SIM_XPLANE;
|
||||
} else {
|
||||
printf("[SIM] Unsupported simulator %s.\n", optarg);
|
||||
fprintf(stderr, "[SIM] Unsupported simulator %s.\n", optarg);
|
||||
}
|
||||
break;
|
||||
|
||||
case 'c':
|
||||
if (!parseMapping(optarg) && sitlSim != SITL_SIM_NONE) {
|
||||
printf("[SIM] Invalid channel mapping string.\n");
|
||||
fprintf(stderr, "[SIM] Invalid channel mapping string.\n");
|
||||
printCmdLineOptions();
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -205,7 +208,11 @@ void parseArguments(int argc, char *argv[])
|
|||
case 'i':
|
||||
simIp = optarg;
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
if (!configFileSetPath(optarg)){
|
||||
fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
|
||||
}
|
||||
break;
|
||||
case 'h':
|
||||
printCmdLineOptions();
|
||||
exit(0);
|
||||
|
|
@ -271,18 +278,18 @@ void delay(timeMs_t ms)
|
|||
|
||||
void systemReset(void)
|
||||
{
|
||||
printf("[SYSTEM] Reset\n");
|
||||
fprintf(stderr, "[SYSTEM] Reset\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void systemResetToBootloader(void)
|
||||
{
|
||||
printf("[SYSTEM] Reset to bootloader\n");
|
||||
fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void failureMode(failureMode_e mode) {
|
||||
printf("[SYSTEM] Failure mode %d\n", mode);
|
||||
fprintf(stderr, "[SYSTEM] Failure mode %d\n", mode);
|
||||
while (1);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue