void info(void) {
AQ_PRINTF("AQ S/N: %08X-%08X-%08X\n", flashSerno(2), flashSerno(1), flashSerno(0));
#ifdef USE_MAVLINK
AQ_PRINTF("Mavlink SYS ID: %d\n", flashSerno(0) % 250);
#endif
AQ_PRINTF("SYS Clock: %u MHz\n", rccClocks.SYSCLK_Frequency / 1000000);
AQ_PRINTF("%u/%u heap used/high water\n", heapUsed, heapHighWater);
AQ_PRINTF("%u of %u CCM heap used\n", dataSramUsed * sizeof(int), UTIL_CCM_HEAP_SIZE * sizeof(int));
yield(50);
utilVersionString();
}
uint8_t navRecordWaypoint(void) {
if (!navData.navCapable || navData.mode >= NAV_STATUS_MISSION)
return 0;
uint8_t idx = 0;
if (navData.tempMissionLoaded)
navClearWaypoints();
else
idx = navGetWaypointCount();
if (idx >= NAV_MAX_MISSION_LEGS)
return 0;
navData.missionLegs[idx].type = NAV_LEG_GOTO;
navData.missionLegs[idx].targetAlt = gpsData.height;
navData.missionLegs[idx].targetLat = gpsData.lat;
navData.missionLegs[idx].targetLon = gpsData.lon;
navData.missionLegs[idx].maxHorizSpeed = NAV_DFLT_HOR_SPEED;
navData.missionLegs[idx].maxVertSpeed = NAV_DFLT_VRT_SPEED;
navData.missionLegs[idx].targetRadius = 1.0f;
navData.missionLegs[idx].loiterTime = 1e6;
navData.missionLegs[idx].poiHeading = AQ_YAW;
navData.missionLegs[idx].relativeAlt = 0;
navData.missionLegs[idx].poiAltitude = 0;
AQ_PRINTF("NAV: Waypoint %d recorded\n", idx);
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_WP_RECORDED);
#endif
#ifdef USE_MAVLINK
mavlinkWpSendCount();
#endif
return 1;
}
static void motorsCanInit(int i) {
uint8_t numTry = CAN_RETRIES;
uint8_t motorId = i;
float escVer;
// canId's 17-32 map to motorId's 1-16
if (motorId >= 16)
motorId -= 16;
while (numTry-- && (motorsData.can[motorId] = canFindNode(CAN_TYPE_ESC, i+1)) == 0)
yield(100);
if (motorsData.can[motorId] == 0) {
AQ_PRINTF("Motors: cannot find CAN id [%d]\n", i+1);
}
else {
#ifdef USE_QUATOS
escVer = esc32SetupCan(motorsData.can[motorId], 1);
#else
escVer = esc32SetupCan(motorsData.can[motorId], 0);
#endif
motorsData.esc32Version[motorId] = (uint16_t)(escVer / 0.01f);
motorsCanRequestTelem(motorId);
}
}
static int motorsPwmInit(int i) {
uint32_t res = PWM_RESOLUTION;
uint32_t freq = MOTORS_PWM_FREQ;
int8_t esc32Mode = -1;
int ret = 1;
if (i >= PWM_NUM_PORTS) {
AQ_PRINTF("Motors: PWM port number %d does not exist!\n", i);
ret = 0;
}
else if (MOTORS_ESC_TYPE == ESC_TYPE_ONBOARD_PWM) {
#if defined(HAS_ONBOARD_ESC)
res = ONBOARD_ESC_PWM_RESOLUTION;
freq = MOTORS_ONBOARD_PWM_FREQ;
#else
AQ_NOTICE("Motors: Onboard ESC not supported on this hardware!\n");
ret = 0;
#endif
}
else if (MOTORS_ESC_TYPE == ESC_TYPE_ESC32) {
esc32Mode = USE_QUATOS;
}
if (ret)
motorsData.pwm[i] = pwmInitOut(i, res, freq, 0, esc32Mode);
return ret;
}
float esc32SetupCan(canNodes_t *canNode, uint8_t mode) {
char *s;
int16_t paramId;
int i = 0;
float ret = 0.0f;
if ((s = canGetVersion(canNode->networkId)) != 0) {
AQ_PRINTF("ESC32: CAN ID: %2d, ver: %s\n", canNode->canId, s);
paramId = canGetParamIdByName(canNode->networkId, (uint8_t *)"STARTUP_MODE");
if (paramId < 0) {
AQ_NOTICE("ESC32: CAN cannot read parameters\n");
}
else if (canGetParamById(canNode->networkId, paramId) != (float)mode) {
if (*canSetRunMode(CAN_TT_NODE, canNode->networkId, mode) == 0)
AQ_NOTICE("ESC32: CAN failed to set run mode\n");
if (*canSetParamById(canNode->networkId, paramId, (float)mode) == 0)
AQ_NOTICE("ESC32: CAN failed to set parameter\n");
else
i++;
}
i += esc32ReadFile(0, canNode);
if (i > 0) {
if (canCommandConfigWrite(CAN_TT_NODE, canNode->networkId)) {
AQ_PRINTF("ESC32: CAN updated %d param(s) in flash\n", i);
}
else {
AQ_NOTICE("ESC32: CAN failed to flash params\n");
}
}
ret = strtof(s, NULL);
}
else {
AQ_PRINTF("ESC32: cannot detect ESC CAN ID %d\n", canNode->canId);
}
if (!isfinite(ret))
ret = 0.0f;
return ret;
}
int configGetParamIdByName(char *name) {
int i = -1;
for (i = 0; i < CONFIG_NUM_PARAMS; i++)
if (!strncmp(name, configParameterStrings[i], 16))
break;
if (i == -1)
AQ_PRINTF("config: cannot find parmeter '%s'\n", name);
return i;
}
void esc32SetupOw(const GPIO_TypeDef *port, const uint16_t pin, uint8_t mode) {
int16_t paramId;
int i = 0;
owInit((GPIO_TypeDef *)port, pin);
// get ESC32 version
owData.buf[0] = OW_VERSION;
owTransaction(1, 16);
if (owData.status != OW_STATUS_NO_PRESENSE) {
AQ_PRINTF("ESC32: OW ver: %s\n", owData.buf);
paramId = esc32OwGetParamId("STARTUP_MODE");
if (paramId < 0) {
AQ_NOTICE("ESC32: OW cannot read parameters\n");
}
else if (esc32OwReadParamById(paramId) != (float)mode) {
if (esc32OwSetMode(mode) != mode)
AQ_NOTICE("ESC32: OW failed to set run mode\n");
if (esc32OwWriteParam(paramId, (float)mode) != (float)mode)
AQ_NOTICE("ESC32: OW failed to write parameter\n");
else
i++;
}
i += esc32ReadFile(0, 0);
if (i > 0) {
esc32OwConfigWrite();
AQ_PRINTF("ESC32: OW updated %d param(s) in flash\n", i);
}
}
else {
AQ_NOTICE("ESC32: cannot detect ESC via OW\n");
}
}
void utilStackCheck(void) {
int i, j;
for (i = 0; i < numStacks; i++) {
for (j = 0; j < stackSizes[i]; j++)
if (*(char *)(stackPointers[i]+j) != 0xFF)
break;
if (stackFrees[i] > j)
stackFrees[i] = j;
if (j < 16) {
AQ_PRINTF("Possible stack overflow [%s]!\n", stackNames[i]);
}
}
}
void motorsEscPwmCalibration(void) {
// check esc type and calibration request bit
if (MOTORS_ESC_TYPE != ESC_TYPE_STD_PWM || !((uint32_t)p[MOT_ESC_TYPE] & MOT_ESC_TYPE_CALIB_ENABLE))
return;
// reset calibration request bit
p[MOT_ESC_TYPE] = (uint32_t)p[MOT_ESC_TYPE] & ~(MOT_ESC_TYPE_CALIB_ENABLE);
// store new param, or fail
if (!configSaveParamsToFlash())
return;
for (int i = 5; i > 0; --i) {
AQ_PRINTF("Warning: ESC calibration, full throttle in %d sec!\n", i);
yield(1000);
}
AQ_NOTICE("ESC Calibration: Setting maximum.\n");
motorsPwmToAll(p[MOT_MAX]);
yield(4000);
AQ_NOTICE("ESC Calibration: Setting minimum.\n");
motorsPwmToAll(p[MOT_MIN]);
yield(3000);
AQ_NOTICE("ESC Calibration: Finished. Restart system now.\n");
}
void utilVersionString(void) {
AQ_PRINTF("AQ FW ver: %s rev%d b%d, HW ver: %d rev%d\n", FIMRWARE_VERSION, getRevisionNumber(), getBuildNumber(), BOARD_VERSION, BOARD_REVISION);
}
void utilSerialNoString(void) {
AQ_PRINTF("AQ S/N: %08X-%08X-%08X\n", flashSerno(2), flashSerno(1), flashSerno(0));
}
void navNavigate(void) {
unsigned long currentTime = IMU_LASTUPD;
unsigned char leg = navData.missionLeg;
navMission_t *curLeg = &navData.missionLegs[leg];
int reqFlightMode; // requested flight mode based on user controls or other factors
float tmp;
navSetFixType();
// is there sufficient position accuracy to navigate?
if (navData.fixType == 3)
navData.navCapable = 1;
// do not drop out of mission due to (hopefully) temporary GPS degradation
else if (navData.mode < NAV_STATUS_POSHOLD)
navData.navCapable = 0;
bool hasViableMission = (navData.navCapable && ((navData.mode != NAV_STATUS_MISSION && leg < NAV_MAX_MISSION_LEGS && curLeg->type) || (navData.mode == NAV_STATUS_MISSION && navData.hasMissionLeg)));
// this defines the hierarchy of available flight modes in case of failsafe override or conflicting controls being active
if (navData.spvrModeOverride)
reqFlightMode = navData.spvrModeOverride;
else if (rcIsSwitchActive(NAV_CTRL_MISN))
if (hasViableMission)
reqFlightMode = NAV_STATUS_MISSION;
else
reqFlightMode = NAV_STATUS_POSHOLD;
else if (rcIsSwitchActive(NAV_CTRL_PH)) {
reqFlightMode = NAV_STATUS_POSHOLD;
}
else if (rcIsSwitchActive(NAV_CTRL_AH))
reqFlightMode = NAV_STATUS_ALTHOLD;
// always default to manual
else
reqFlightMode = NAV_STATUS_MANUAL;
// Can we navigate && do we want to be in mission mode?
if ((supervisorData.state & STATE_ARMED) && reqFlightMode == NAV_STATUS_MISSION && hasViableMission) {
// are we currently in position hold mode && do we have a clear mission ahead of us?
if (navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) {
curLeg = navLoadLeg(leg);
navData.mode = NAV_STATUS_MISSION;
AQ_NOTICE("Mission mode active\n");
}
}
// do we want to be in position hold mode?
else if ((supervisorData.state & STATE_ARMED) && reqFlightMode > NAV_STATUS_MANUAL) {
// check for DVH
if (reqFlightMode == NAV_STATUS_POSHOLD && (RADIO_PITCH > p[CTRL_DEAD_BAND] || RADIO_PITCH < -p[CTRL_DEAD_BAND] || RADIO_ROLL > p[CTRL_DEAD_BAND] || RADIO_ROLL < -p[CTRL_DEAD_BAND]))
reqFlightMode = NAV_STATUS_DVH;
// allow alt hold regardless of GPS or flow quality
if (navData.mode < NAV_STATUS_ALTHOLD || (navData.mode != NAV_STATUS_ALTHOLD && reqFlightMode == NAV_STATUS_ALTHOLD)) {
// record this altitude as the hold altitude
navSetHoldAlt(ALTITUDE, 0);
// set integral to current RC throttle setting
pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);
navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
navData.mode = NAV_STATUS_ALTHOLD;
// notify ground
AQ_NOTICE("Altitude Hold engaged\n");
}
// are we not in position hold mode now?
if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode > NAV_STATUS_ALTHOLD && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
// only zero bias if coming from lower mode
if (navData.mode < NAV_STATUS_POSHOLD) {
navData.holdTiltN = 0.0f;
navData.holdTiltE = 0.0f;
// speed
pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);
// pos
pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
}
// store this position as hold position
navUkfSetHereAsPositionTarget();
if (navData.navCapable) {
// set this position as home if we have none
if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
navSetHomeCurrent();
}
// activate pos hold
navData.mode = NAV_STATUS_POSHOLD;
navData.holdSpeedN = 0.0f;
navData.holdSpeedE = 0.0f;
navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
// notify ground
AQ_NOTICE("Position Hold engaged\n");
}
// DVH
else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode == NAV_STATUS_DVH) {
navData.mode = NAV_STATUS_DVH;
}
// coming out of DVH mode?
else if (navData.mode == NAV_STATUS_DVH) {
// allow speed to drop before holding position (or if RTH engaged)
// FIXME: RTH switch may no longer be engaged but craft is still returning to home?
if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || rcIsSwitchActive(NAV_CTRL_HOM_GO)) {
navUkfSetHereAsPositionTarget();
navData.mode = NAV_STATUS_POSHOLD;
}
}
}
// default to manual mode
else {
if (navData.mode != NAV_STATUS_MANUAL)
AQ_NOTICE("Manual mode active.\n");
navData.mode = NAV_STATUS_MANUAL;
// reset mission legs
navData.missionLeg = leg = 0;
// keep up with changing altitude
navSetHoldAlt(ALTITUDE, 0);
}
// ceiling set ?, 0 is disable
if (navData.ceilingAlt) {
// ceiling reached 1st time
if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
navData.setCeilingFlag = 1;
navData.ceilingTimer = timerMicros();
}
// ceiling still reached for 5 seconds
else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
navData.ceilingTimer = timerMicros();
if (!navData.setCeilingReached)
AQ_NOTICE("Warning: Altitude ceiling reached.");
navData.setCeilingReached = 1;
}
else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
if (navData.setCeilingReached)
AQ_NOTICE("Notice: Altitude returned to within ceiling.");
navData.setCeilingFlag = 0;
navData.setCeilingReached = 0;
}
}
if (!(supervisorData.state & STATE_RADIO_LOSS1))
navDoUserCommands(&leg, curLeg);
if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
}
else {
navData.holdCourse = 0.0f;
navData.holdDistance = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// have we arrived yet?
if (navData.loiterCompleteTime == 0) {
// are we close enough (distance and altitude)?
// goto/home test
if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
// orbit test
(curLeg->type == NAV_LEG_ORBIT &&
fabsf(navData.holdDistance - curLeg->targetRadius) <= 2.0f &&
fabsf(navData.holdAlt - ALTITUDE) <= 1.0f) ||
// takeoff test
(curLeg->type == NAV_LEG_TAKEOFF &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
) {
// freeze heading if relative, unless orbiting
if (curLeg->type != NAV_LEG_ORBIT && signbit(navData.targetHeading))
navData.targetHeading = AQ_YAW;
// start the loiter clock
navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
AQ_PRINTF("NAV: Reached waypoint %d.\n", leg);
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_WP_REACHED);
#endif
#ifdef USE_MAVLINK
// notify ground
mavlinkWpReached(leg);
#endif
}
}
// have we loitered long enough?
else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
// next leg
leg = ++navData.missionLeg;
curLeg = navLoadLeg(leg);
if (!navData.hasMissionLeg)
navData.mode = NAV_STATUS_POSHOLD;
}
}
// DVH
if (navData.mode == NAV_STATUS_DVH) {
float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
float x = 0.0f;
float y = 0.0f;
if (RADIO_PITCH > p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL > p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;
// do we want to ignore rotation of craft (headfree/carefree mode)?
if (navData.headFreeMode > NAV_HEADFREE_OFF) {
if (navData.hfUseStoredReference) {
// rotate to stored frame
navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
}
else {
// don't rotate to any frame (pitch/roll move to N/S/E/W)
navData.holdSpeedN = x;
navData.holdSpeedE = y;
}
}
else {
// rotate to earth frame
navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
}
}
// orbit POI
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
float velX, velY;
// maintain orbital radius
velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);
// maintain orbital velocity (clock wise)
velY = -curLeg->maxHorizSpeed;
// rotate to earth frame
navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
}
else {
// distance => velocity
navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
}
// normalize N/E speed requests to fit below max nav speed
tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
if (tmp > navData.holdMaxHorizSpeed) {
navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
}
// velocity => tilt
navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);
if (navData.mode > NAV_STATUS_MANUAL) {
float vertStick;
// Throttle controls vertical speed
vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
if ((vertStick > p[CTRL_DBAND_THRO] && !navData.setCeilingReached) || vertStick < -p[CTRL_DBAND_THRO]) {
// altitude velocity proportional to throttle stick
navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * NAV_DFLT_VRT_SPEED * (1.0f / RADIO_MID_THROTTLE);
navData.verticalOverride = 1;
}
// are we trying to land?
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
}
// coming out of vertical override?
else if (navData.verticalOverride) {
navData.targetHoldSpeedAlt = 0.0f;
// slow down before trying to hold altitude
if (fabsf(VELOCITYD) < 0.025f)
navData.verticalOverride = 0;
// set new hold altitude to wherever we are while still in override
if (navData.mode != NAV_STATUS_MISSION)
navSetHoldAlt(ALTITUDE, 0);
}
// PID has the throttle
else {
navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
}
// constrain vertical velocity
tmp = configGetParamValue(NAV_MAX_DECENT);
tmp = (navData.holdMaxVertSpeed < tmp) ? -navData.holdMaxVertSpeed : -tmp;
navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, tmp, navData.holdMaxVertSpeed);
// smooth vertical velocity changes
navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
}
else {
navData.verticalOverride = 0;
}
// calculate POI angle (used for tilt in gimbal function)
if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
float a, b, c;
a = navData.holdDistance;
b = ALTITUDE - curLeg->poiAltitude;
c = __sqrtf(a*a + b*b);
navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
}
else {
navData.poiAngle = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// recalculate autonomous heading
navSetHoldHeading(navData.targetHeading);
// wait for low throttle if landing
if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
// shut everything down (sure hope we are really on the ground :)
supervisorDisarm();
}
navData.lastUpdate = currentTime;
}
navMission_t *navLoadLeg(uint8_t leg) {
navMission_t *curLeg = &navData.missionLegs[leg];
// invalid type?
if (!curLeg->type || curLeg->type >= NAV_NUM_LEG_TYPES) {
navData.hasMissionLeg = 0;
return &navData.missionLegs[0];
}
// common
if (curLeg->relativeAlt)
navSetHoldAlt(curLeg->targetAlt, curLeg->relativeAlt);
else
navSetHoldAlt(curLeg->targetAlt - navData.presAltOffset, curLeg->relativeAlt);
navData.holdMaxHorizSpeed = curLeg->maxHorizSpeed;
navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
// type specific
if (curLeg->type == NAV_LEG_HOME) {
navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
navData.targetHeading = navData.homeLeg.poiHeading;
navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
}
else if (curLeg->type == NAV_LEG_GOTO) {
if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
navData.targetHeading = curLeg->poiHeading;
}
else if (curLeg->type == NAV_LEG_ORBIT) {
if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
navData.targetHeading = -0.0f; // must point to target
}
else if (curLeg->type == NAV_LEG_TAKEOFF) {
// store this position as the takeoff position
navUkfSetHereAsPositionTarget();
navData.targetHeading = AQ_YAW;
navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
// set the launch location as home
navSetHomeCurrent();
navData.homeLeg.targetAlt = navData.holdAlt;
navData.homeLeg.poiHeading = -0.0f; // relative
}
else if (curLeg->type == NAV_LEG_LAND) {
navUkfSetHereAsPositionTarget(); // hold current position
navData.targetHeading = fabsf(curLeg->poiHeading); // always fixed
navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
}
if (navData.holdMaxHorizSpeed == 0.0f)
navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
if (navData.holdMaxVertSpeed == 0.0f)
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
navData.loiterCompleteTime = 0;
navData.hasMissionLeg = 1;
navData.missionLeg = leg;
AQ_PRINTF("NAV: Loaded waypoint %d.\n", leg);
#ifdef USE_MAVLINK
// notify ground
mavlinkWpAnnounceCurrent(leg);
#endif
return curLeg;
}
void radioInit(void) {
uint16_t radioType = (uint16_t)p[RADIO_TYPE];
int i;
AQ_NOTICE("Radio init\n");
memset((void *)&radioData, 0, sizeof(radioData));
radioData.mode = (radioType>>12) & 0x0f;
for (i = 0; i < RADIO_NUM; i++) {
radioInstance_t *r = &radioData.radioInstances[i];
USART_TypeDef *uart;
// determine UART
switch (i) {
case 0:
uart = RC1_UART;
break;
#ifdef RC2_UART
case 1:
uart = RC2_UART;
break;
#endif
#ifdef RC3_UART
case 2:
uart = RC3_UART;
break;
#endif
default:
uart = 0;
break;
}
r->radioType = (radioType>>(i*4)) & 0x0f;
r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i];
utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f);
switch (r->radioType) {
case RADIO_TYPE_SPEKTRUM11:
case RADIO_TYPE_SPEKTRUM10:
case RADIO_TYPE_DELTANG:
if (uart) {
spektrumInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("Spektrum on RC port %d\n", i);
}
break;
case RADIO_TYPE_SBUS:
if (uart) {
futabaInit(r, uart);
radioRCSelect(i, 1);
AQ_PRINTF("Futaba on RC port %d\n", i);
}
break;
case RADIO_TYPE_PPM:
ppmInit(r);
AQ_PRINTF("PPM on RC port %d\n", i);
break;
case RADIO_TYPE_SUMD:
if (uart) {
grhottInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("GrHott on RC port %d\n", i);
}
break;
case RADIO_TYPE_MLINK:
if (uart) {
mlinkrxInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("Mlink on RC port %d\n", i);
}
break;
case RADIO_TYPE_NONE:
break;
default:
AQ_NOTICE("WARNING: Invalid radio type!\n");
break;
}
}
switch (radioData.mode) {
case RADIO_MODE_DIVERSITY:
// select first available radio to start with
for (i = 0; i < RADIO_NUM; i++) {
if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) {
radioMakeCurrent(&radioData.radioInstances[i]);
break;
}
}
break;
case RADIO_MODE_SPLIT:
radioMakeCurrent(&radioData.radioInstances[0]);
break;
}
// set mode default
radioData.channels[(int)p[RADIO_FLAP_CH]] = -700;
radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO");
radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE);
}
