// set headfree mode based on radio command
void navSetHeadFreeMode(void) {
// HF switch to set/dynamic position
// when disarmed one can also set the orientation heading in this position (for 2-pos control)
if (rcIsSwitchActive(NAV_CTRL_HF_SET) || (!(supervisorData.state & STATE_ARMED) && navData.headFreeMode != NAV_HEADFREE_LOCKED && rcIsSwitchActive(NAV_CTRL_HF_LOCK))) {
if (navData.headFreeMode != NAV_HEADFREE_DYNAMIC) {
if (navData.headFreeMode != NAV_HEADFREE_SETTING && navData.headFreeMode != NAV_HEADFREE_DYN_DELAY) {
navData.headFreeMode = NAV_HEADFREE_SETTING;
navData.hfDynamicModeTimer = timerMicros();
AQ_NOTICE("Set new reference heading for Heading-Free mode\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_HF_SET);
#endif
}
else if ((supervisorData.state & STATE_ARMED) && timerMicros() - navData.hfDynamicModeTimer > NAV_HF_DYNAMIC_DELAY) {
navData.headFreeMode = NAV_HEADFREE_DYNAMIC;
AQ_NOTICE("Now in dynamic Heading-Free DVH mode\n");
}
else if (navData.headFreeMode == NAV_HEADFREE_SETTING)
navData.headFreeMode = NAV_HEADFREE_DYN_DELAY;
}
}
// HF switch to locked/on position
else if (rcIsSwitchActive(NAV_CTRL_HF_LOCK)) {
if (navData.headFreeMode != NAV_HEADFREE_LOCKED) {
AQ_NOTICE("Now in locked Heading-Free DVH mode\n");
navData.headFreeMode = NAV_HEADFREE_LOCKED;
}
}
// HF channel at off position
else {
if (navData.headFreeMode > NAV_HEADFREE_DYN_DELAY)
AQ_NOTICE("Now in normal DVH mode\n");
navData.headFreeMode = NAV_HEADFREE_OFF;
}
}
// write config to uSD
int8_t configWriteFile(char *fname) {
char buf[128];
int8_t fh;
int8_t ret;
int n;
int i;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get write file handle\n");
return -1;
}
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
n = configFormatParam(buf, i);
ret = filerWrite(fh, buf, -1, n);
if (ret < n) {
AQ_NOTICE("config: file write error\n");
ret = -1;
break;
}
}
filerClose(fh);
return ret;
}
void signalingInit(void) {
memset((void *)&sigData, 0, sizeof(sigData));
sigData.patPos = 0;
sigData.patLen = 10; // 10 = 1Hz. Changing it will! affect the led pattern event
if (p[SIG_LED_1_PRT]) {
sigData.ledPort1 = pwmInitDigitalOut(p[SIG_LED_1_PRT]-1);
if (sigData.ledPort1)
sigData.enabled = 1;
else
AQ_NOTICE("Warning: Could not open LED 1 signaling port!\n");
}
if (p[SIG_LED_2_PRT]) {
sigData.ledPort2 = pwmInitDigitalOut(p[SIG_LED_2_PRT]-1);
if (sigData.ledPort2)
sigData.enabled = 1;
else
AQ_NOTICE("Warning: Could not open LED 2 signaling port!\n");
}
if (p[SIG_BEEP_PRT]) {
if (p[SIG_BEEP_PRT] > 0) { // piezo buzzer
sigData.beeperPort = pwmInitDigitalOut(abs(p[SIG_BEEP_PRT])-1);
sigData.beeperType = 0;
}
else { // piezo speaker
sigData.beeperPort = pwmInitOut(abs(p[SIG_BEEP_PRT])-1, 200.0f / SIG_SPEAKER_FREQ * 5000, 0, 0);
sigData.beeperType = 1;
}
if (sigData.beeperPort)
sigData.enabled = 1;
else
AQ_NOTICE("Warning: Could not open Beeper signaling port!\n");
}
if (p[SIG_PWM_PRT]) {
sigData.pwmPort = pwmInitOut(p[SIG_PWM_PRT]-1, 2500, 950, 0);
if (sigData.pwmPort)
sigData.enabled = 1;
else
AQ_NOTICE("Warning: Could not open PWM signaling port!\n");
}
signalingWriteOutput(SIG_EVENT_NONE);
// beep the cell count
if (sigData.beeperPort) {
int i;
for (i = 0; i < analogData.batCellCount; i++) {
signalingBeep(2000, 50);
delay(150);
}
delay(300);
signalingBeep(2000, 250); // ready beep
}
}
void supervisorArm(void) {
if (motorsArm()) {
supervisorData.state = STATE_ARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
AQ_NOTICE("Armed\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_ARMING);
#endif
}
else {
motorsDisarm();
AQ_NOTICE("Arm motors failed - disarmed\n");
}
}
void controlInit(void) {
AQ_NOTICE("Control init\n");
memset((void *)&controlData, 0, sizeof(controlData));
#ifdef USE_QUATOS
quatosInit(AQ_INNER_TIMESTEP, AQ_OUTER_TIMESTEP);
#endif
utilFilterInit3(controlData.userPitchFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
utilFilterInit3(controlData.userRollFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
utilFilterInit3(controlData.navPitchFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);
utilFilterInit3(controlData.navRollFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);
controlData.pitchRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
&p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
controlData.rollRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
&p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
controlData.yawRatePID = pidInit(&p[CTRL_YAW_RTE_P], &p[CTRL_YAW_RTE_I], &p[CTRL_YAW_RTE_D], &p[CTRL_YAW_RTE_F], &p[CTRL_YAW_RTE_PM],
&p[CTRL_YAW_RTE_IM], &p[CTRL_YAW_RTE_DM], &p[CTRL_YAW_RTE_OM], 0, 0, 0, 0);
controlData.pitchAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
&p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
controlData.rollAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
&p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
controlData.yawAnglePID = pidInit(&p[CTRL_YAW_ANG_P], &p[CTRL_YAW_ANG_I], &p[CTRL_YAW_ANG_D], &p[CTRL_YAW_ANG_F], &p[CTRL_YAW_ANG_PM],
&p[CTRL_YAW_ANG_IM], &p[CTRL_YAW_ANG_DM], &p[CTRL_YAW_ANG_OM], 0, 0, 0, 0);
controlTaskStack = aqStackInit(CONTROL_STACK_SIZE, "CONTROL");
controlData.controlTask = CoCreateTask(controlTaskCode, (void *)0, CONTROL_PRIORITY, &controlTaskStack[CONTROL_STACK_SIZE-1],
CONTROL_STACK_SIZE);
}
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;
}
// read config from uSD
int8_t configReadFile(char *fname) {
char *fileBuf;
int8_t fh;
int ret;
int p1;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get read file handle\n");
return -1;
}
fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char));
p1 = 0;
while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0)
p1 = configParseParams((char *)fileBuf, ret, p1);
filerClose(fh);
if (fileBuf)
aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));
return ret;
}
void supervisorTare(void) {
supervisorLEDsOn();
#ifndef USE_SIMU
dIMUTare();
#endif
AQ_NOTICE("Level calibration complete.\n");
supervisorLEDsOff();
}
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;
}
void commandInit(void) {
AQ_NOTICE("Command interface init\n");
memset((void *)&commandData, 0, sizeof(commandData));
commRegisterRcvrFunc(COMM_STREAM_TYPE_TELEMETRY, commandTaskCode);
commandData.state = COMMAND_WAIT_SYNC1;
}
// write config to uSD
int8_t configWriteFile(char *fname) {
char *buf;
int8_t fh;
int8_t ret;
int n;
int i;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get write file handle\n");
return -1;
}
if (!(buf = (char *)aqCalloc(128, sizeof(char)))) {
AQ_NOTICE("config: Error writing to file, cannot allocate memory.\n");
filerClose(fh);
return -1;
}
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
n = configFormatParam(buf, i);
if (n)
ret = filerWrite(fh, buf, -1, n);
if (!n || ret < n) {
ret = -1;
break;
}
}
filerClose(fh);
if (buf)
aqFree(buf, 128, sizeof(char));
if (ret > -1)
AQ_NOTICE("config: Parameters saved to local storage file.\n");
else
AQ_NOTICE("config: Error writing parameters to file.\n");
return ret;
}
void supervisorDisarm(void) {
motorsDisarm();
calibDeinit();
supervisorLEDsOff();
supervisorData.state = STATE_DISARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
AQ_NOTICE("Disarmed\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_DISARMING);
#endif
}
static void dIMUReadCalib(void) {
#ifdef DIMU_HAVE_EEPROM
uint8_t *buf;
int size;
int p1 = 0;
buf = eepromOpenRead();
if (buf == 0) {
AQ_NOTICE("DIMU: cannot read EEPROM parameters!\n");
}
else {
while ((size = eepromRead(DIMU_EEPROM_BLOCK_SIZE)) != 0)
p1 = configParseParams((char *)buf, size, p1);
AQ_NOTICE("DIMU: read calibration parameters from EEPROM\n");
}
#endif
}
// read config from uSD
int8_t configReadFile(char *fname) {
char *fileBuf;
int8_t fh;
int ret;
int p1;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get read file handle\n");
return -1;
}
if (!(fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char)))) {
AQ_NOTICE("config: Error reading from file, cannot allocate memory.\n");
filerClose(fh);
return -1;
}
p1 = 0;
while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0) {
p1 = configParseParams((char *)fileBuf, ret, p1);
if (p1 < 0) {
ret = -1;
break;
}
}
filerClose(fh);
if (fileBuf)
aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));
if (ret > -1)
AQ_NOTICE("config: Parameters loaded from local storage file.\n");
else
AQ_NOTICE("config: Failed to read parameters from local file.");
return ret;
}
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 configFlashRead(void) {
configRec_t *recs;
int i, j;
recs = (void *)flashStartAddr();
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
for (j = 0; j < CONFIG_NUM_PARAMS; j++)
if (!strncasecmp(recs[i].name, configParameterStrings[j], 16))
p[j] = recs[i].val;
}
AQ_NOTICE("config: Parameters restored from flash memory.\n");
}
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");
}
// allocates memory from 64KB CCM
void *aqDataCalloc(uint16_t count, uint16_t size) {
uint32_t words;
// round up to word size
words = (count*size + sizeof(int)-1) / sizeof(int);
if ((dataSramUsed + words) > UTIL_CCM_HEAP_SIZE) {
AQ_NOTICE("Out of data SRAM!\n");
}
else {
dataSramUsed += words;
}
return (void *)(ccmHeap + dataSramUsed - words);
}
static void dIMUWriteCalib(void) {
#ifdef DIMU_HAVE_EEPROM
char *lineBuf;
uint8_t *buf;
int n;
int i, j, k;
if (!(lineBuf = (char *)aqCalloc(128, sizeof(char)))) {
AQ_NOTICE("DIMU: Error writing to EEPROM, cannot allocate memory.\n");
return;
}
buf = eepromOpenWrite();
k = 0;
for (i = 0; i < sizeof(dImuCalibParameters) / sizeof(uint16_t); i++) {
n = configFormatParam(lineBuf, dImuCalibParameters[i]);
for (j = 0; j < n; j++) {
buf[k++] = lineBuf[j];
if (k == DIMU_EEPROM_BLOCK_SIZE) {
eepromWrite();
k = 0;
}
}
}
if (k != 0)
eepromWrite();
if (lineBuf)
aqFree(lineBuf, 128, sizeof(char));
AQ_NOTICE("DIMU: wrote calibration parameters to EEPROM\n");
eepromClose();
#endif
}
void *aqCalloc(size_t count, size_t size) {
char *addr = 0;
if (count * size) {
addr = calloc(count, size);
heapUsed += count * size;
if (heapUsed > heapHighWater)
heapHighWater = heapUsed;
if (addr == 0)
AQ_NOTICE("Out of heap memory!\n");
}
return addr;
}
void navSetHomeCurrent(void) {
navData.homeLeg.type = NAV_LEG_GOTO;
navData.homeLeg.relativeAlt = 0;
navData.homeLeg.targetAlt = ALTITUDE;
navData.homeLeg.targetLat = gpsData.lat;
navData.homeLeg.targetLon = gpsData.lon;
navData.homeLeg.maxHorizSpeed = p[NAV_MAX_SPEED];
navData.homeLeg.poiHeading = AQ_YAW;
if (p[NAV_CEILING])
navData.ceilingAlt = (ALTITUDE + p[NAV_CEILING]); // home altitude + x meter as ceiling
// notify ground
AQ_NOTICE("Home position set\n");
#ifdef USE_MAVLINK
mavlinkAnnounceHome();
#endif
}
unsigned char navClearWaypoints(void) {
if (navData.mode >= NAV_STATUS_MISSION)
return 0;
for (int i = 0; i < NAV_MAX_MISSION_LEGS; i++)
navData.missionLegs[i].type = 0;
navData.tempMissionLoaded = 0;
AQ_NOTICE("NAV: Waypoints cleared.\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_WP_CLEARED);
#endif
#ifdef USE_MAVLINK
mavlinkWpSendCount();
#endif
return 1;
}
void navInit(void) {
int i = 0;
AQ_NOTICE("Nav init\n");
memset((void *)&navData, 0, sizeof(navData));
navData.speedNPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
navData.speedEPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
navData.distanceNPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
navData.distanceEPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
navData.altSpeedPID = pidInit(&p[NAV_ALT_SPED_P], &p[NAV_ALT_SPED_I], 0, 0, &p[NAV_ALT_SPED_PM], &p[NAV_ALT_SPED_IM], 0,
&p[NAV_ALT_SPED_OM], 0, 0, 0, 0);
navData.altPosPID = pidInit(&p[NAV_ALT_POS_P], &p[NAV_ALT_POS_I], 0, 0, &p[NAV_ALT_POS_PM], &p[NAV_ALT_POS_IM], 0, &p[NAV_ALT_POS_OM], 0, 0,
0, 0);
navData.mode = NAV_STATUS_MANUAL;
navData.ceilingAlt = 0.0f;
navData.setCeilingFlag = 0;
navData.setCeilingReached = 0;
navData.homeActionFlag = 0;
navData.distanceToHome = 0.0f;
navData.headFreeMode = NAV_HEADFREE_OFF;
navData.hfUseStoredReference = 0;
navSetHoldHeading(AQ_YAW);
navSetHoldAlt(ALTITUDE, 0);
// HOME
navData.missionLegs[i].type = NAV_LEG_HOME;
navData.missionLegs[i].targetRadius = 0.10f;
navData.missionLegs[i].loiterTime = 0;
navData.missionLegs[i].poiAltitude = ALTITUDE;
i++;
// land
navData.missionLegs[i].type = NAV_LEG_LAND;
navData.missionLegs[i].maxHorizSpeed = 1.0f;
navData.missionLegs[i].poiAltitude = 0.0f;
i++;
}
void navInit(void) {
AQ_NOTICE("Nav init\n");
memset((void *)&navData, 0, sizeof(navData));
navData.speedNPID = pidInit(NAV_SPEED_P, NAV_SPEED_I, 0, 0, NAV_SPEED_PM, NAV_SPEED_IM, 0, NAV_SPEED_OM);
navData.speedEPID = pidInit(NAV_SPEED_P, NAV_SPEED_I, 0, 0, NAV_SPEED_PM, NAV_SPEED_IM, 0, NAV_SPEED_OM);
navData.distanceNPID = pidInit(NAV_DIST_P, NAV_DIST_I, 0, 0, NAV_DIST_PM, NAV_DIST_IM, 0, NAV_DIST_OM);
navData.distanceEPID = pidInit(NAV_DIST_P, NAV_DIST_I, 0, 0, NAV_DIST_PM, NAV_DIST_IM, 0, NAV_DIST_OM);
navData.altSpeedPID = pidInit(NAV_ALT_SPED_P, NAV_ALT_SPED_I, 0, 0, NAV_ALT_SPED_PM, NAV_ALT_SPED_IM, 0, NAV_ALT_SPED_OM);
navData.altPosPID = pidInit(NAV_ALT_POS_P, NAV_ALT_POS_I, 0, 0, NAV_ALT_POS_PM, NAV_ALT_POS_IM, 0, NAV_ALT_POS_OM);
navData.mode = NAV_STATUS_MANUAL;
navData.headFreeMode = NAV_HEADFREE_OFF;
navSetHoldHeading(AQ_YAW);
navSetHoldAlt(ALTITUDE, 0);
navLoadDefaultMission();
}
void radioTaskCode(void *unused) {
int i;
AQ_NOTICE("Radio task started\n");
while (1) {
// wait for data
yield(2); // 2ms
for (i = 0; i < RADIO_NUM; i++) {
radioInstance_t *r = &radioData.radioInstances[i];
if (r->radioType > RADIO_TYPE_NONE) {
radioProcessInstance(r);
// find best signal
if (radioData.mode == RADIO_MODE_DIVERSITY && r->quality > *radioData.quality)
radioMakeCurrent(r);
}
}
}
}
void supervisorArm(void) {
uint8_t rcStat = rcCheckValidController();
if (rcStat) {
AQ_NOTICE("Error: Can't arm due to RC error(s):\n");
rcReportAllErrors(rcStat);
}
else if (rcIsSwitchActive(NAV_CTRL_AH) || rcIsSwitchActive(NAV_CTRL_PH) || rcIsSwitchActive(NAV_CTRL_MISN))
AQ_NOTICE("Error: Can't arm, not in manual flight mode.\n");
else if (rcIsSwitchActive(NAV_CTRL_HOM_SET) || rcIsSwitchActive(NAV_CTRL_HOM_GO))
AQ_NOTICE("Error: Can't arm, home command active.\n");
else if (rcIsSwitchActive(NAV_CTRL_HF_SET) || rcIsSwitchActive(NAV_CTRL_HF_LOCK))
AQ_NOTICE("Error: Can't arm, heading-free mode active.\n");
else if (motorsArm()) {
supervisorData.state = STATE_ARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
AQ_NOTICE("Armed\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_ARMING);
#endif
}
else {
motorsDisarm();
AQ_NOTICE("Error: Arm motors failed - disarmed.\n");
}
}
void supervisorTaskCode(void *unused) {
unsigned long lastAqCounter = 0; // used for idle time calc
uint32_t count = 0;
AQ_NOTICE("Supervisor task started\n");
// wait for ADC vIn data
while (analogData.batCellCount == 0)
yield(100);
supervisorData.vInLPF = analogData.vIn;
if (analogData.extAmp > 0.0f)
supervisorData.currentSenseValPtr = &analogData.extAmp;
else if (canSensorsData.values[CAN_SENSORS_PDB_BATA] > 0.0f)
supervisorData.currentSenseValPtr = &canSensorsData.values[CAN_SENSORS_PDB_BATA];
else
supervisorData.aOutLPF = SUPERVISOR_INVALID_AMPSOUT_VALUE;
if (supervisorData.currentSenseValPtr)
supervisorData.aOutLPF = *supervisorData.currentSenseValPtr;
while (1) {
yield(1000/SUPERVISOR_RATE);
if (supervisorData.state & STATE_CALIBRATION) {
int i;
// try to indicate completion percentage
i = constrainInt(20*((calibData.percentComplete)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.readyLed);
else if (!(count % i))
digitalTogg(supervisorData.readyLed);
#ifdef SUPERVISOR_DEBUG_PORT
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.debugLed);
else if (!(count % i))
digitalTogg(supervisorData.debugLed);
#endif //SUPERVISOR_DEBUG_PORT
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f*2.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.gpsLed);
else if (!(count % i))
digitalTogg(supervisorData.gpsLed);
// user looking to go back to DISARMED mode?
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
} // end if calibrating
else if (supervisorData.state & STATE_DISARMED) {
#ifdef SUPERVISOR_DEBUG_PORT
// 0.5 Hz blink debug LED if config file could be found on uSD card
if (supervisorData.configRead && (!(count % SUPERVISOR_RATE))) {
// only for first 15s
if (timerMicros() > 15000000) {
supervisorData.configRead = 0;
digitalLo(supervisorData.debugLed);
}
else {
digitalTogg(supervisorData.debugLed);
}
}
#endif // SUPERVISOR_DEBUG_PORT
// 1 Hz blink if disarmed, 5 Hz if writing to uSD card
if (!(count % ((supervisorData.diskWait) ? SUPERVISOR_RATE/10 : SUPERVISOR_RATE/2)))
digitalTogg(supervisorData.readyLed);
// Attempt to arm if throttle down and yaw full right for 2sec.
if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD > +500) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorArm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
// various functions
if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
if (!supervisorData.stickCmdTimer) {
supervisorData.stickCmdTimer = timerMicros();
} else if ((timerMicros() - supervisorData.stickCmdTimer) > SUPERVISOR_STICK_CMD_TIME) {
#ifdef HAS_DIGITAL_IMU
// tare function (lower left)
if (RADIO_ROLL < -500 && RADIO_PITCH > +500) {
supervisorTare();
supervisorData.stickCmdTimer = 0;
}
else
#endif // HAS_DIGITAL_IMU
// config write (upper right)
if (RADIO_ROLL > +500 && RADIO_PITCH < -500) {
supervisorLEDsOn();
configSaveParamsToFlash();
#ifdef DIMU_HAVE_EEPROM
dIMURequestCalibWrite();
#endif
supervisorLEDsOff();
supervisorData.stickCmdTimer = 0;
}
// calibration mode (upper left)
else if (RADIO_ROLL < -500 && RADIO_PITCH < -500) {
supervisorCalibrate();
supervisorData.stickCmdTimer = 0;
}
// clear waypoints (lower right with WP Record switch active)
else if (RADIO_ROLL > 500 && RADIO_PITCH > 500 && rcIsSwitchActive(NAV_CTRL_WP_REC)) {
navClearWaypoints();
supervisorData.stickCmdTimer = 0;
}
} // end stick timer check
}
// no stick commands detected
else
supervisorData.stickCmdTimer = 0;
} // end if disarmed
else if (supervisorData.state & STATE_ARMED) {
// Disarm only if in manual mode
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500 && navData.mode == NAV_STATUS_MANUAL) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
}
// end of calibrating/disarmed/armed mode checks
// radio loss
if ((supervisorData.state & STATE_FLYING) && (navData.mode < NAV_STATUS_MISSION || (supervisorData.state & STATE_RADIO_LOSS2))) {
// regained?
if (RADIO_QUALITY > 1.0f) {
supervisorData.lastGoodRadioMicros = timerMicros();
if (supervisorData.state & STATE_RADIO_LOSS1)
AQ_NOTICE("Warning: radio signal regained\n");
navData.spvrModeOverride = 0;
supervisorData.state &= ~(STATE_RADIO_LOSS1 | STATE_RADIO_LOSS2);
}
// loss 1
else if (!(supervisorData.state & STATE_RADIO_LOSS1) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS1) {
supervisorData.state |= STATE_RADIO_LOSS1;
AQ_NOTICE("Warning: Radio loss stage 1 detected\n");
// hold position
navData.spvrModeOverride = NAV_STATUS_POSHOLD;
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE; // center throttle
}
// loss 2
else if (!(supervisorData.state & STATE_RADIO_LOSS2) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS2) {
supervisorData.state |= STATE_RADIO_LOSS2;
AQ_NOTICE("Warning: Radio loss stage 2! Initiating recovery.\n");
// only available with GPS
if (navData.navCapable) {
navMission_t *wp;
uint8_t wpi = 0;
uint8_t fsOption = (uint8_t)p[SPVR_FS_RAD_ST2];
navClearWaypoints();
wp = navGetWaypoint(wpi++);
if (fsOption > SPVR_OPT_FS_RAD_ST2_LAND && navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon) > SUPERVISOR_HOME_POS_DETECT_RADIUS) {
float targetAltitude;
// ascend
if (fsOption == SPVR_OPT_FS_RAD_ST2_ASCEND && ALTITUDE < navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT]) {
// the home leg's altitude is recorded without pressure offset
targetAltitude = navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT] + navData.presAltOffset;
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = gpsData.lat;
wp->targetLon = gpsData.lon;
wp->targetRadius = SUPERVISOR_HOME_ALT_DETECT_MARGIN;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
else {
// the greater of our current altitude or home's altitude
targetAltitude = ((ALTITUDE > navData.homeLeg.targetAlt) ? ALTITUDE : navData.homeLeg.targetAlt) + navData.presAltOffset;
}
// go home with previously determined altitude
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = navData.homeLeg.targetLat;
wp->targetLon = navData.homeLeg.targetLon;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
// decend to home
wp->type = NAV_LEG_HOME;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
// land
wp->type = NAV_LEG_LAND;
wp->maxVertSpeed = NAV_DFLT_LND_SPEED;
wp->maxHorizSpeed = 0.0f;
wp->poiAltitude = 0.0f;
// go
navData.missionLeg = 0;
navData.tempMissionLoaded = 1;
navData.spvrModeOverride = NAV_STATUS_MISSION;
}
// no GPS, slow decent in PH mode
else {
navData.spvrModeOverride = NAV_STATUS_POSHOLD;
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE * 3 / 4; // 1/4 max decent
}
}
}
// end radio loss check
// calculate idle time
supervisorData.idlePercent = (counter - lastAqCounter) * minCycles * 100.0f / ((1e6f / SUPERVISOR_RATE) * rccClocks.SYSCLK_Frequency / 1e6f);
lastAqCounter = counter;
// smooth vIn readings
supervisorData.vInLPF += (analogData.vIn - supervisorData.vInLPF) * (0.1f / SUPERVISOR_RATE);
// smooth current flow readings, if any
if (supervisorData.currentSenseValPtr)
supervisorData.aOutLPF += (*supervisorData.currentSenseValPtr - supervisorData.aOutLPF) * (0.1f / SUPERVISOR_RATE);
//calculate remaining battery % based on configured low batt stg 2 level -- ASSumes 4.2v/cell maximum
supervisorData.battRemainingPrct = (supervisorData.vInLPF - p[SPVR_LOW_BAT2] * analogData.batCellCount) / ((4.2f - p[SPVR_LOW_BAT2]) * analogData.batCellCount) * 100;
// low battery
if (!(supervisorData.state & STATE_LOW_BATTERY1) && supervisorData.vInLPF < (p[SPVR_LOW_BAT1]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY1;
AQ_NOTICE("Warning: Low battery stage 1\n");
// TODO: something
}
else if (!(supervisorData.state & STATE_LOW_BATTERY2) && supervisorData.vInLPF < (p[SPVR_LOW_BAT2]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY2;
AQ_NOTICE("Warning: Low battery stage 2\n");
// TODO: something
}
// end battery level checks
supervisorSetSystemStatus();
if (supervisorData.state & STATE_FLYING) {
// count flight time in seconds
supervisorData.flightTime += (1.0f / SUPERVISOR_RATE);
// rapidly flash Ready LED if we are critically low on power
if (supervisorData.state & STATE_LOW_BATTERY2)
digitalTogg(supervisorData.readyLed);
}
else if (supervisorData.state & STATE_ARMED) {
digitalHi(supervisorData.readyLed);
}
#ifdef SUPERVISOR_DEBUG_PORT
// DEBUG LED to indicate radio RX state
if (!supervisorData.configRead && RADIO_INITIALIZED && supervisorData.state != STATE_CALIBRATION) {
// packet received in the last 50ms?
if (RADIO_VALID) {
digitalHi(supervisorData.debugLed);
}
else {
if (RADIO_BINDING)
digitalTogg(supervisorData.debugLed);
else
digitalLo(supervisorData.debugLed);
}
}
#endif
count++;
#ifdef USE_SIGNALING
signalingEvent();
#endif
}
}
void supervisorTare(void) {
supervisorLEDsOn();
dIMUTare();
AQ_NOTICE("Level calibration complete.\n");
supervisorLEDsOff();
}
void supervisorCalibrate(void) {
supervisorData.state = STATE_CALIBRATION;
AQ_NOTICE("Starting MAG calibration mode.\n");
calibInit();
}
void runTaskCode(void *unused) {
uint32_t axis = 0;
uint32_t loops = 0;
AQ_NOTICE("Run task started\n");
while (1) {
// wait for data
CoWaitForSingleFlag(imuData.sensorFlag, 0);
// soft start GPS accuracy
runData.accMask *= 0.999f;
navUkfInertialUpdate();
// record history for acc & mag & pressure readings for smoothing purposes
// acc
runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];
runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;
runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];
// mag
runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];
runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;
runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];
// pressure
runData.sumPres -= runData.presHist[runData.sensorHistIndex];
runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
runData.sumPres += runData.presHist[runData.sensorHistIndex];
runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;
if (!((loops+1) % 20)) {
simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
else if (!((loops+7) % 20)) {
simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
}
#ifndef USE_DIGITAL_IMU
else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
#endif
// optical flow update
else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
navUkfFlowUpdate();
}
// only accept GPS updates if there is no optical flow
else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
CoClearFlag(gpsData.gpsPosFlag);
// refine static sea level pressure based on better GPS altitude fixes
if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
navPressureAdjust(gpsData.height);
runData.bestHacc = gpsData.hAcc;
}
}
else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
CoClearFlag(gpsData.gpsVelFlag);
}
// observe zero position
else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroPos();
}
// observer zero velocity
else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroVel();
}
// observe that the rates are exactly 0 if not flying or moving
else if (!(supervisorData.state & STATE_FLYING)) {
float stdX, stdY, stdZ;
arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);
if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
if (!((axis + 0) % 3))
navUkfZeroRate(IMU_RATEX, 0);
else if (!((axis + 1) % 3))
navUkfZeroRate(IMU_RATEY, 1);
else
navUkfZeroRate(IMU_RATEZ, 2);
axis++;
}
}
navUkfFinish();
altUkfProcess(AQ_PRESSURE);
// determine which altitude estimate to use
if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
runData.altPos = &ALT_POS;
runData.altVel = &ALT_VEL;
}
else {
runData.altPos = &UKF_ALTITUDE;
runData.altVel = &UKF_VELD;
}
CoSetFlag(runData.runFlag); // new state data
navNavigate();
#ifndef HAS_AIMU
analogDecode();
#endif
if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
loggerDoHeader();
loggerDo();
gimbalUpdate();
#ifdef CAN_CALIB
canTxIMUData(loops);
#endif
calibrate();
loops++;
}
}
