void STDMIXER_SetChannelOrderByProtocol()
{
const u8 *ch_map = CurrentProtocolChannelMap;
if (! ch_map) {
// for none protocol, assign any channel to thr is fine
ch_map = EATRG0;
}
CLOCK_ResetWatchdog();// this function might be invoked after loading from template/model file, so feeding the dog in the middle
unsigned safetysw = 0;
s8 safetyval = 0;
for (unsigned ch = 0; ch < 3; ch++) { // only the first 3 channels need to check
if (Model.limits[ch].safetysw) {
safetysw = Model.limits[ch].safetysw;
safetyval = Model.limits[ch].safetyval;
}
if (ch_map[ch] == INP_THROTTLE)
mapped_std_channels.throttle = ch;
else if (ch_map[ch] == INP_AILERON)
mapped_std_channels.actual_aile = mapped_std_channels.aile = ch;
else if (ch_map[ch] == INP_ELEVATOR)
mapped_std_channels.actual_elev = mapped_std_channels.elev = ch;
}
Model.limits[mapped_std_channels.throttle].safetysw = safetysw;
Model.limits[mapped_std_channels.throttle].safetyval = safetyval;
Model.limits[mapped_std_channels.aile].safetysw = 0;
Model.limits[mapped_std_channels.elev].safetysw = 0;
Model.limits[mapped_std_channels.aile].safetyval = 0;
Model.limits[mapped_std_channels.elev].safetyval = 0;
//printf("thro: %d, aile: %d, elev: %d\n\n", mapped_std_channels.throttle, mapped_std_channels.aile, mapped_std_channels.elev);
MIXER_SetTemplate(mapped_std_channels.throttle, MIXERTEMPLATE_COMPLEX);
MIXER_SetTemplate(mapped_std_channels.aile, MIXERTEMPLATE_CYC1);
MIXER_SetTemplate(mapped_std_channels.elev, MIXERTEMPLATE_CYC2);
struct Mixer *mix = MIXER_GetAllMixers();
for (unsigned idx = 0; idx < NUM_MIXERS; idx++) {
if (mix[idx].src ==0)
continue;
if (mix[idx].dest == NUM_OUT_CHANNELS + 9)
mix[idx].src = 0; // remove all mixers pointing to Virt10, because the Virt10 is reserved in Standard mode
else if (MIXER_MUX(&mix[idx]) == MUX_REPLACE && mix[idx].src== INP_THROTTLE && mix[idx].dest < NUM_OUT_CHANNELS) { // src=THR && dest = virt should be pitch's mixer
mix[idx].dest = mapped_std_channels.throttle;
}
}
MIXER_SetTemplate(NUM_OUT_CHANNELS + 9, MIXERTEMPLATE_NONE);// remove all mixers pointing to Virt10 as the Virt10 is reserved in Standard mode
mapped_std_channels.aile = NUM_OUT_CHANNELS; // virt 1
mapped_std_channels.elev = NUM_OUT_CHANNELS +1; // virt 2
// Simplfied timer sw, only throttle channel output is possible to be selected
for (unsigned i = 0; i < NUM_TIMERS; i++) {
if (Model.timer[i].src)
Model.timer[i].src = mapped_std_channels.throttle + NUM_INPUTS +1;
TIMER_Reset(i);
}
CLOCK_ResetWatchdog();
}
static void wait_release()
{
printf("Wait Release\n");
while(1) {
CLOCK_ResetWatchdog();
u32 buttons = ScanButtons();
if (! CHAN_ButtonIsPressed(buttons, BUT_ENTER))
break;
if(PWR_CheckPowerSwitch())
PWR_Shutdown();
}
printf("Released\n");
}
void EventLoop()
{
CLOCK_ResetWatchdog();
unsigned int time;
#ifdef HEAP_DEBUG
static int heap = 0;
int h = _sbrk_r(NULL, 0);
if(h > heap) {
printf("heap: %x\n", h);
heap = h;
}
#endif
#ifdef TIMING_DEBUG
debug_timing(0, 0);
#endif
priority_ready &= ~(1 << MEDIUM_PRIORITY);
if(PWR_CheckPowerSwitch()) {
if(! (BATTERY_Check() & BATTERY_CRITICAL)) {
CONFIG_SaveModelIfNeeded();
CONFIG_SaveTxIfNeeded();
}
if(Transmitter.music_shutdown) {
MUSIC_Play(MUSIC_SHUTDOWN);
// We wait ~1sec for shutdown music finished
time = CLOCK_getms()+700;
while(CLOCK_getms()<time);
}
PWR_Shutdown();
}
BUTTON_Handler();
TOUCH_Handler();
if (priority_ready & (1 << LOW_PRIORITY)) {
priority_ready &= ~(1 << LOW_PRIORITY);
PAGE_Event();
PROTOCOL_CheckDialogs();
TIMER_Update();
TELEMETRY_Alarm();
BATTERY_Check();
AUTODIMMER_Update();
#if DATALOG_ENABLED
DATALOG_Update();
#endif
GUI_RefreshScreen();
}
#ifdef TIMING_DEBUG
debug_timing(0, 1);
#endif
}
static void calibrate_sticks(void)
{
// bug fix: should turn of safety dialog during calibrating, or it might fail when stick is not calibrated and safety setting is on
PAGE_DisableSafetyDialog(1);
PROTOCOL_DeInit();
PAGE_SetModal(1);
PAGE_RemoveAllObjects();
PAGE_SetActionCB(_action_cb_calibrate);
snprintf(tempstring, sizeof(tempstring), "%s", _tr("Center all \nsticks and knobs\nthen press ENT"));
GUI_CreateLabelBox(&guic->msg, 1, 10, LCD_WIDTH -1, LCD_HEIGHT - 10,
LCD_HEIGHT > 70? &NARROW_FONT:&DEFAULT_FONT, NULL, NULL, tempstring);
memcpy(cp->calibration, Transmitter.calibration, sizeof(cp->calibration));
while(1) {
CLOCK_ResetWatchdog();
if(PWR_CheckPowerSwitch())
PWR_Shutdown();
if(priority_ready & (1 << MEDIUM_PRIORITY)) {
BUTTON_Handler();
priority_ready &= ~(1 << MEDIUM_PRIORITY);
}
if(priority_ready & (1 << LOW_PRIORITY)) {
//Only sample every 100msec
GUI_RefreshScreen();
priority_ready = 0;
}
for (u8 i = 0; i < INP_HAS_CALIBRATION; i++) {
s32 value = CHAN_ReadRawInput(i + 1);
if (value > Transmitter.calibration[i].max)
Transmitter.calibration[i].max = value;
else if (value < Transmitter.calibration[i].min)
Transmitter.calibration[i].min = value;
}
if (calibrate_state == CALI_SUCCESSEXIT || calibrate_state == CALI_EXIT)
break;
}
if (calibrate_state == CALI_EXIT)
memcpy(Transmitter.calibration, cp->calibration, sizeof(cp->calibration));
PAGE_SetActionCB(NULL);
PROTOCOL_Init(0);
PAGE_TxConfigureInit(-1); // should be -1 so that devo10 can get back to previous item selection
PAGE_DisableSafetyDialog(0);
}
void PAGE_CalibInit(int page)
{
(void)page;
PROTOCOL_DeInit();
PAGE_SetActionCB(_action_cb_calibrate);
snprintf(tempstring, sizeof(tempstring), "%s", _tr("Center all \nsticks and knobs\nthen press ENT"));
GUI_CreateLabelBox(&guic->msg, 1, CALIB_Y, 0, 0,
LCD_HEIGHT > 70? &NARROW_FONT:&DEFAULT_FONT, NULL, NULL, tempstring);
memcpy(cp->calibration, Transmitter.calibration, sizeof(cp->calibration));
while(1) {
CLOCK_ResetWatchdog();
if(PWR_CheckPowerSwitch())
PWR_Shutdown();
if(priority_ready & (1 << MEDIUM_PRIORITY)) {
BUTTON_Handler();
priority_ready &= ~(1 << MEDIUM_PRIORITY);
}
if(priority_ready & (1 << LOW_PRIORITY)) {
//Only sample every 100msec
GUI_RefreshScreen();
priority_ready = 0;
}
for (u8 i = 0; i < INP_HAS_CALIBRATION; i++) {
s32 value = CHAN_ReadRawInput(i + 1);
if (value > Transmitter.calibration[i].max)
Transmitter.calibration[i].max = value;
else if (value < Transmitter.calibration[i].min)
Transmitter.calibration[i].min = value;
}
if (calibrate_state == CALI_SUCCESSEXIT || calibrate_state == CALI_EXIT)
break;
}
if (calibrate_state == CALI_EXIT)
memcpy(Transmitter.calibration, cp->calibration, sizeof(cp->calibration));
PAGE_Pop();
// PAGE_SetActionCB(NULL);
// PROTOCOL_Init(0);
// PAGE_SetModal(0);
// //cp->enable = CALIB_NONE;
// PAGE_ChangeByID(PAGEID_TXCFG, 0);
}
static int hubsan_init()
{
u8 if_calibration1;
u8 vco_calibration0;
u8 vco_calibration1;
//u8 vco_current;
A7105_WriteID(0x55201041);
A7105_WriteReg(A7105_01_MODE_CONTROL, 0x63);
A7105_WriteReg(A7105_03_FIFOI, 0x0f);
A7105_WriteReg(A7105_0D_CLOCK, 0x05);
A7105_WriteReg(A7105_0E_DATA_RATE, 0x04);
A7105_WriteReg(A7105_15_TX_II, 0x2b);
A7105_WriteReg(A7105_18_RX, 0x62);
A7105_WriteReg(A7105_19_RX_GAIN_I, 0x80);
A7105_WriteReg(A7105_1C_RX_GAIN_IV, 0x0A);
A7105_WriteReg(A7105_1F_CODE_I, 0x07);
A7105_WriteReg(A7105_20_CODE_II, 0x17);
A7105_WriteReg(A7105_29_RX_DEM_TEST_I, 0x47);
A7105_Strobe(A7105_STANDBY);
//IF Filter Bank Calibration
A7105_WriteReg(0x02, 1);
//vco_current =
A7105_ReadReg(0x02);
u32 ms = CLOCK_getms();
CLOCK_ResetWatchdog();
while(CLOCK_getms() - ms < 500) {
if(! A7105_ReadReg(0x02))
break;
}
if (CLOCK_getms() - ms >= 500) {
DEBUG_MSG("calibration failed");
return 0;
}
if_calibration1 = A7105_ReadReg(A7105_22_IF_CALIB_I);
A7105_ReadReg(A7105_24_VCO_CURCAL);
if(if_calibration1 & A7105_MASK_FBCF) {
//Calibration failed...what do we do?
return 0;
}
//VCO Current Calibration
//A7105_WriteReg(0x24, 0x13); //Recomended calibration from A7105 Datasheet
//VCO Bank Calibration
//A7105_WriteReg(0x26, 0x3b); //Recomended limits from A7105 Datasheet
//VCO Bank Calibrate channel 0?
//Set Channel
A7105_WriteReg(A7105_0F_CHANNEL, 0);
//VCO Calibration
A7105_WriteReg(0x02, 2);
ms = CLOCK_getms();
CLOCK_ResetWatchdog();
while(CLOCK_getms() - ms < 500) {
if(! A7105_ReadReg(0x02))
break;
}
if (CLOCK_getms() - ms >= 500) {
return 0;
}
vco_calibration0 = A7105_ReadReg(A7105_25_VCO_SBCAL_I);
if (vco_calibration0 & A7105_MASK_VBCF) {
//Calibration failed...what do we do?
return 0;
}
//Calibrate channel 0xa0?
//Set Channel
A7105_WriteReg(A7105_0F_CHANNEL, 0xa0);
//VCO Calibration
A7105_WriteReg(A7105_02_CALC, 2);
ms = CLOCK_getms();
CLOCK_ResetWatchdog();
while(CLOCK_getms() - ms < 500) {
if(! A7105_ReadReg(A7105_02_CALC))
break;
}
if (CLOCK_getms() - ms >= 500)
return 0;
vco_calibration1 = A7105_ReadReg(A7105_25_VCO_SBCAL_I);
if (vco_calibration1 & A7105_MASK_VBCF) {
//Calibration failed...what do we do?
}
//Reset VCO Band calibration
//A7105_WriteReg(0x25, 0x08);
A7105_SetTxRxMode(TX_EN);
A7105_SetPower(Model.tx_power);
A7105_Strobe(A7105_STANDBY);
return 1;
}
static int layout_ini_handler(void* user, const char* section, const char* name, const char* value)
{
struct Model *m = (struct Model *)user;
u16 i;
int offset_x = 0, offset_y = 0;
CLOCK_ResetWatchdog();
int idx;
if (MATCH_START(name, GUI_QUICKPAGE)) {
u8 idx = name[9] - '1';
if (idx >= NUM_QUICKPAGES) {
printf("%s: Only %d quickpages are supported\n", section, NUM_QUICKPAGES);
return 1;
}
int max = PAGE_GetNumPages();
for(i = 0; i < max; i++) {
if(mapstrcasecmp(PAGE_GetName(i), value) == 0) {
m->pagecfg2.quickpage[idx] = i;
return 1;
}
}
printf("%s: Unknown page '%s' for quickpage%d\n", section, value, idx+1);
return 1;
}
#ifdef ENABLE_320x240_GUI
static u8 seen_res = 0;
enum {
LOWRES = 1,
HIRES,
};
if (! MATCH_SECTION(SECTION_GUI)) {
if(MATCH_SECTION("gui-320x240")
&& (! ELEM_USED(Model.pagecfg2.elem[0]) || seen_res != HIRES))
{
seen_res = LOWRES;
offset_x = (LCD_WIDTH - 320) / 2;
offset_y = (LCD_HEIGHT - 240) / 2;
} else
return 1;
} else {
if (seen_res == LOWRES) {
memset(&Model.pagecfg2.elem, 0, sizeof(Model.pagecfg2.elem));
}
seen_res = HIRES;
}
#else
if (! MATCH_SECTION(SECTION_GUI))
return 1;
#endif
for (idx = 0; idx < NUM_ELEMS; idx++) {
if (! ELEM_USED(Model.pagecfg2.elem[idx]))
break;
}
if (idx == NUM_ELEMS) {
printf("No free element available (max = %d)\n", NUM_ELEMS);
return 1;
}
int type;
for (type = 0; type < ELEM_LAST; type++)
if(mapstrcasecmp(name, GetElemName(type)) == 0)
break;
if (type == ELEM_LAST)
return 1;
int count = 5;
s16 data[6] = {0};
const char *ptr = parse_partial_int_list(value, data, &count, S16);
data[0] += offset_x;
data[1] += offset_y;
if (count > 3) {
printf("Could not parse coordinates from %s=%s\n", name,value);
return 1;
}
switch(type) {
//case ELEM_MODEL: //x, y
case ELEM_VTRIM: //x, y, src
case ELEM_HTRIM: //x, y, src
data[5] = data[2];
data[2] = 0;
break;
case ELEM_SMALLBOX: //x, y, src
case ELEM_BIGBOX: //x. y. src
{
s16 src = -1;
char str[20];
if (count != 3)
return 1;
#if HAS_RTC
for(i = 0; i < NUM_RTC; i++) {
if(mapstrcasecmp(ptr, RTC_Name(str, i)) == 0) {
src = i + 1;
break;
}
}
#endif
if (src == -1) {
for(i = 0; i < NUM_TIMERS; i++) {
if(mapstrcasecmp(ptr, TIMER_Name(str, i)) == 0) {
src = i + 1 + NUM_RTC;
break;
}
}
}
if (src == -1) {
for(i = 0; i < NUM_TELEM; i++) {
if(mapstrcasecmp(ptr, TELEMETRY_Name(str, i+1)) == 0) {
src = i + 1 + NUM_RTC + NUM_TIMERS;
break;
}
}
}
if (src == -1) {
u8 newsrc = get_source(section, ptr);
if(newsrc >= NUM_INPUTS) {
src = newsrc - (NUM_INPUTS + 1 - (NUM_RTC + NUM_TIMERS + NUM_TELEM + 1));
}
}
if (src == -1)
src = 0;
data[5] = src;
break;
}
case ELEM_BAR: //x, y, src
{
if (count != 3)
return 1;
u8 src = get_source(section, ptr);
if (src < NUM_INPUTS)
src = 0;
data[5] = src - NUM_INPUTS;
break;
}
case ELEM_TOGGLE: //x, y, tgl0, tgl1, tgl2, src
{
if(count)
return 1;
for (int j = 0; j <= NUM_SOURCES; j++) {
char cmp[10];
if(mapstrcasecmp(INPUT_SourceNameAbbrevSwitchReal(cmp, j), ptr+1) == 0) {
data[5] = j;
break;
}
}
break;
}
}
create_element(&m->pagecfg2.elem[idx], type, data);
return 1;
}
void EventLoop()
{
CLOCK_ResetWatchdog();
#ifdef HEAP_DEBUG
static int heap = 0;
int h = _sbrk_r(NULL, 0);
if(h > heap) {
printf("heap: %x\n", h);
heap = h;
}
#endif
#ifdef TIMING_DEBUG
debug_timing(0, 0);
#endif
priority_ready &= ~(1 << MEDIUM_PRIORITY);
#if !HAS_HARD_POWER_OFF
if(PWR_CheckPowerSwitch()) {
if(! (BATTERY_Check() & BATTERY_CRITICAL)) {
PAGE_Test();
CONFIG_SaveModelIfNeeded();
CONFIG_SaveTxIfNeeded();
}
if(Transmitter.music_shutdown) {
#if HAS_EXTENDED_AUDIO
if(AUDIO_VoiceAvailable()) {
MUSIC_Play(MUSIC_SHUTDOWN);
while (CLOCK_getms() < audio_queue_time) {
// Wait for voice to finished
CLOCK_ResetWatchdog();
}
} else {
#else
{
// We wait ~1sec for shutdown buzzer music finished
unsigned int time;
MUSIC_Play(MUSIC_SHUTDOWN);
time = CLOCK_getms()+700;
while (CLOCK_getms() < time) {
CLOCK_ResetWatchdog();
}
#endif
}
}
PWR_Shutdown();
}
#endif
BUTTON_Handler();
TOUCH_Handler();
INPUT_CheckChanges();
if (priority_ready & (1 << LOW_PRIORITY)) {
priority_ready &= ~(1 << LOW_PRIORITY);
PAGE_Event();
PROTOCOL_CheckDialogs();
TIMER_Update();
TELEMETRY_Alarm();
BATTERY_Check();
AUTODIMMER_Update();
#if HAS_DATALOG
DATALOG_Update();
#endif
#if HAS_VIDEO
VIDEO_Update();
#endif
#if HAS_EXTENDED_AUDIO
AUDIO_CheckQueue();
#endif
GUI_RefreshScreen();
#if HAS_HARD_POWER_OFF
if (PAGE_ModelDoneEditing())
CONFIG_SaveModelIfNeeded();
CONFIG_SaveTxIfNeeded();
#endif
}
#ifdef TIMING_DEBUG
debug_timing(0, 1);
#endif
}
void TOUCH_Handler() {
if(! HAS_TOUCH)
return;
u32 pen_down=0;
static u32 pen_down_last=0;
static u32 pen_down_long_at=0;
struct touch t;
if(SPITouch_IRQ()) {
pen_down=1;
t=SPITouch_GetCoords();
if (! pen_down_last)
pen_down_long_at=CLOCK_getms()+500;
} else {
pen_down=0;
}
if(pen_down && (!pen_down_last)) {
AUTODIMMER_Check();
GUI_CheckTouch(&t, 0);
}
if(!pen_down && pen_down_last) {
GUI_TouchRelease();
}
if(pen_down && pen_down_last) {
if(CLOCK_getms()>pen_down_long_at) {
GUI_CheckTouch(&t, 1);
pen_down_long_at += 100;
}
}
pen_down_last=pen_down;
}
#if HAS_VIDEO
void VIDEO_Update()
{
static u8 video_enable = 0;
static u32 check_standard_ms = 0;
// Check if Video is turn on
int enabled = MIXER_SourceAsBoolean(Model.videosrc);
if (enabled != video_enable) {
VIDEO_Enable(enabled);
video_enable = enabled;
if (enabled) {
VIDEO_SetChannel(Model.videoch);
VIDEO_Contrast(Model.video_contrast);
VIDEO_Brightness(Model.video_brightness);
check_standard_ms = CLOCK_getms() + 3000;
}
else
check_standard_ms = 0;
}
if(video_enable &&
check_standard_ms > 0
&& check_standard_ms < CLOCK_getms()) {
u8 video_standard_current = VIDEO_GetStandard();
if((video_standard_current > 0) &&
(video_standard_current < 8)) {
VIDEO_SetStandard(video_standard_current);
check_standard_ms = 0;
}
else {
check_standard_ms = CLOCK_getms() + 3000;
}
}
if(video_enable)
AUTODIMMER_Check();
}
#endif //HAS_VIDEO
#ifdef TIMING_DEBUG
void debug_timing(u32 type, int startend)
{
static u32 last_time[2][100];
static u32 loop_time[4][101];
static u32 loop_pos[4] = {-1, -1, -1, -1};
static u32 max_last[2];
static u32 max_loop[4];
static int save_priority;
if (type == 0) {
if (! startend)
save_priority = priority_ready;
if (save_priority & (1 << MEDIUM_PRIORITY))
debug_timing(2, startend);
if (save_priority & (1 << LOW_PRIORITY))
debug_timing(1, startend);
return;
}
type--;
if (! startend) {
u32 t = CLOCK_getms();
loop_pos[type] = (loop_pos[type] + 1) % 100;
if (type < 2) {
last_time[type][loop_pos[type]] = t;
if (t - last_time[type][(loop_pos[type] + 99) % 100] > max_last[type])
max_last[type] = t - last_time[type][(loop_pos[type] + 99) % 100];
}
loop_time[type][100] = t;
} else {
loop_time[type][loop_pos[type]] = CLOCK_getms() - loop_time[type][100];
if (loop_time[type][loop_pos[type]] > max_loop[type])
max_loop[type] = loop_time[type][loop_pos[type]];
if (type == 0 && loop_pos[0] == 99) {
unsigned avg_loop[4] = {0, 0, 0, 0};
unsigned avg_last[2] = {0, 0};
for(int i = 0; i < 99; i++) {
for(int t = 0; t < 2; t++) {
u32 delay = last_time[t][(i + loop_pos[t] + 2) % 100] - last_time[t][(i + loop_pos[t] + 1) % 100];
avg_last[t] += delay;
}
for(int t = 0; t < 4; t++)
avg_loop[t] += loop_time[t][i];
}
for(int t = 0; t < 4; t++)
avg_loop[t] /= 99;
avg_last[0] /= 99;
avg_last[1] /= 99;
printf("Avg: radio: %d mix: %d med: %d/%d low: %d/%d\n", avg_loop[3], avg_loop[2], avg_loop[1], avg_last[1], avg_loop[0], avg_last[0]);
printf("Max: radio: %d mix: %d med: %d/%d low: %d/%d\n", max_loop[3], max_loop[2], max_loop[1], max_last[1], max_loop[0], max_last[0]);
memset(max_loop, 0, sizeof(max_loop));
max_last[0] = 0;
max_last[1] = 0;
}
}
}
#endif
void debug_switches()
{
s32 data[INP_LAST];
for(int i = INP_HAS_CALIBRATION+1; i < INP_LAST; i++) {
data[i] = CHAN_ReadRawInput(i);
}
while(1) {
u32 changed = 0;
for(int i = INP_HAS_CALIBRATION+1; i < INP_LAST; i++) {
s32 val = CHAN_ReadRawInput(i);
if (val != data[i]) {
printf("%s=%d ", INPUT_SourceName(tempstring, i), val);
data[i] = val;
changed = 1;
}
}
if (changed) { printf("\n"); }
if(PWR_CheckPowerSwitch()) PWR_Shutdown();
}
}