Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Fixed issue then bailing out of RTH to PH and position was not reset (drone would still be going home). Fixed conditionless activation of WP mode (required GPS, BARO and ARMED now). FC version string changed. Possible fix to MAG mode (untested).

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-28 14:12:41 +10:00
parent e4f327d4b7
commit fa901e9b9b
4 changed files with 34 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/flight/navigation_rewrite.c
View file

@ -36,6 +36,7 @@
#include "sensors/boardalignment.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/imu.h"
@ -512,7 +513,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState)
{
if ((navGetStateFlags(previousState) & NAV_CTL_POS) == 0) {
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
t_fp_vector targetHoldPos;
resetPositionController();
@ -531,7 +534,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
{
if ((navGetStateFlags(previousState) & NAV_CTL_ALT) == 0) {
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
resetAltitudeController();
setupAltitudeController();
@ -540,7 +545,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
}
if ((navGetStateFlags(previousState) & NAV_CTL_POS) == 0) {
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
t_fp_vector targetHoldPos;
resetPositionController();
@ -1588,12 +1593,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
#if defined(NAV_BLACKBOX)
navFlags = 0;
if (posControl.flags.verticalPositionNewData) navFlags |= (1 << 0);
if (posControl.flags.horizontalPositionNewData) navFlags |= (1 << 1);
if (posControl.flags.headingNewData) navFlags |= (1 << 2);
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 3);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 4);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 5);
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
if ((STATE(GPS_FIX) && GPS_numSat >= 5)) navFlags |= (1 << 3);
#endif
// No navigation when disarmed
@ -1679,7 +1682,8 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
if (canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {

2
src/main/io/serial_cli.c
View file

@ -2411,7 +2411,7 @@ static void cliVersion(char *cmdline)
{
UNUSED(cmdline);
printf("# Cleanflight/%s %s %s / %s (%s)",
printf("# iNav/%s %s %s / %s (%s)",
targetName,
FC_VERSION_STRING,
buildDate,

3
src/main/io/serial_msp.c
View file

@ -139,10 +139,11 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define MULTIWII_IDENTIFIER "MWII";
#define CLEANFLIGHT_IDENTIFIER "CLFL"
#define INAV_IDENTIFIER "INAV"
#define BASEFLIGHT_IDENTIFIER "BAFL";
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF

44
src/main/mw.c
View file

@ -575,36 +575,26 @@ void processRx(void)
}
#if defined(MAG)
#if defined(NAV)
if (naivationGetHeadingControlState() != NAV_HEADING_CONTROL_NONE) {
DISABLE_FLIGHT_MODE(MAG_MODE);
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
else {
#endif
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
}
#if defined(NAV)
}
#endif
#endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {