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
}
}
static void motorsPwmInit(int i) {
#if defined(HAS_ONBOARD_ESC)
motorsData.pwm[i] = pwmInitOut(i, HAS_ONBOARD_ESC, MOTORS_PWM_FREQ, 0, 0);
#else
#if defined(USE_QUATOS)
motorsData.pwm[i] = pwmInitOut(i, 1000000, MOTORS_PWM_FREQ, 0, 1); // closed loop RPM mode
#else
motorsData.pwm[i] = pwmInitOut(i, 1000000, MOTORS_PWM_FREQ, 0, 0); // open loop mode
#endif // USE_QUATOS
#endif // HAS_ONBOARD_ESC
}
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;
}
void gimbalInit(void) {
uint16_t pwmPeriod;
int8_t chkTim, initPitchPort, initRollPort, initTiltPort, initTrigPort, initPsthrPort;
AQ_NOTICE("Gimbal init\n");
memset((void *)&gimbalData, 0, sizeof(gimbalData));
gimbalData.trigger = 0;
gimbalData.triggerCount = 0;
gimbalData.triggerLogVal = 0;
gimbalData.triggerTimer = 0;
gimbalData.triggerLastTime = 0;
gimbalData.triggerLastLat = (double)0.0f;
gimbalData.triggerLastLon = (double)0.0f;
if (!p[GMBL_PWM_FREQ]) {
AQ_NOTICE("Gimbal functions disabled.\n");
return;
}
chkTim = ((int)p[GMBL_PWM_FREQ] != MOTORS_PWM_FREQ);
initPitchPort = (p[GMBL_PITCH_PORT] && (!chkTim || !pwmCheckTimer(p[GMBL_PITCH_PORT]-1)));
initRollPort = (p[GMBL_ROLL_PORT] && (!chkTim || !pwmCheckTimer(p[GMBL_ROLL_PORT]-1)));
initTiltPort = (p[GMBL_TILT_PORT] && p[GMBL_TILT_PORT] != p[GMBL_PITCH_PORT] && (!chkTim || !pwmCheckTimer(p[GMBL_TILT_PORT]-1)));
initTrigPort = (p[GMBL_TRIG_PORT] && (!chkTim || !pwmCheckTimer(p[GMBL_TRIG_PORT]-1)));
initPsthrPort = (p[GMBL_PSTHR_PORT] && (!chkTim || !pwmCheckTimer(p[GMBL_PSTHR_PORT]-1)) && p[GMBL_PSTHR_CHAN] && (int)p[GMBL_PSTHR_CHAN] <= RADIO_MAX_CHANNELS);
pwmPeriod = PWM_PRESCALE / (int)p[GMBL_PWM_FREQ];
if (initPitchPort) {
gimbalData.pitchPort = pwmInitOut(p[GMBL_PITCH_PORT]-1, pwmPeriod, p[GMBL_NTRL_PITCH], -1);
if (gimbalData.pitchPort) {
if (p[GMBL_TILT_PORT] == p[GMBL_PITCH_PORT])
AQ_NOTICE("Gimbal combined PITCH & TILT port initialized.\n");
else
AQ_NOTICE("Gimbal PITCH stabilization port initialized.\n");
yield(100);
}
}
if (initRollPort) {
gimbalData.rollPort = pwmInitOut(p[GMBL_ROLL_PORT]-1, pwmPeriod, p[GMBL_NTRL_ROLL], -1);
if (gimbalData.rollPort) {
AQ_NOTICE("Gimbal ROLL stabilization port initialized.\n");
}
}
if (initTiltPort) {
gimbalData.tiltPort = pwmInitOut(p[GMBL_TILT_PORT]-1, pwmPeriod, p[GMBL_NTRL_PITCH], -1);
if (gimbalData.tiltPort) {
AQ_NOTICE("Gimbal TILT control port initialized.\n");
}
}
if (initTrigPort) {
gimbalData.triggerPort = pwmInitOut(p[GMBL_TRIG_PORT]-1, pwmPeriod, GMBL_TRIG_NTRL_PWM, -1);
if (gimbalData.triggerPort) {
AQ_NOTICE("Gimbal TRIGGER control port initialized.\n");
}
}
if (initPsthrPort) {
gimbalData.passthroughPort = pwmInitOut(p[GMBL_PSTHR_PORT]-1, pwmPeriod, GMBL_TRIG_NTRL_PWM, -1);
if (gimbalData.passthroughPort) {
AQ_NOTICE("Gimbal radio PASSTHROUGH port initialized.\n");
}
}
}
