// 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++; } }