Skip to content

Commit

Permalink
Merge pull request #117 from ligenxxxx/optimize-boot-0mw
Browse files Browse the repository at this point in the history 
add paralized mode; detect FC alive
  • Loading branch information
@ligenxxxx
ligenxxxx committed Apr 17, 2023
2 parents 56acf76 + abcc625 commit a5e769d
Show file tree
Hide file tree
Showing 5 changed files with 74 additions and 42 deletions.
25 changes: 17 additions & 8 deletions src/hardware.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,15 +41,12 @@ uint8_t RF_FREQ = 0;
uint8_t LP_MODE = 0;
uint8_t PIT_MODE = 0;
uint8_t OFFSET_25MW = 0; // 0~10 -> 0~10 11~20 -> -1~-10
uint8_t BOOT_0MW = 0;
uint8_t TEAM_RACE = 0;
uint8_t BAUDRATE = 0;

uint8_t RF_BW = BW_27M;
uint8_t RF_BW_last = BW_27M;

uint8_t g_IS_ARMED = 0;
uint8_t g_IS_PARALYZE = 0;

uint8_t cfg_step = 0; // 0:idle, 1:freq, 2:power, 3:LP_MODE

uint8_t cfg_to_cnt = 0;
Expand Down Expand Up @@ -284,7 +281,7 @@ void Setting_Save() {
err |= I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_PITMODE, PIT_MODE);
err |= I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_25MW, OFFSET_25MW);
err |= I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_BAUDRATE, BAUDRATE);
err |= I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_BOOT_0MW, BOOT_0MW);
err |= I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_TEAM_RACE, TEAM_RACE);
#ifdef _DEBUG_MODE
if (!err)
debugf("\r\nEEPROM write success");
Expand All @@ -299,7 +296,7 @@ void Setting_Save() {
debugf(" PIT_MODE=%d\r\n", (uint16_t)PIT_MODE);
debugf(" OFFSET_25MW=%d\r\n", (uint16_t)OFFSET_25MW);
debugf(" BAUDRATE=%d\r\n", (uint16_t)BAUDRATE);
debugf(" BOOT_0MW=%d\r\n", (uint16_t)BOOT_0MW);
debugf(" TEAM_RACE=%d\r\n", (uint16_t)TEAM_RACE);
#endif
}

Expand All @@ -309,7 +306,7 @@ void CFG_Back() {
LP_MODE = (LP_MODE > 2) ? 0 : LP_MODE;
PIT_MODE = (PIT_MODE > PIT_0MW) ? PIT_OFF : PIT_MODE;
OFFSET_25MW = (OFFSET_25MW > 20) ? 0 : OFFSET_25MW;
BOOT_0MW = (BOOT_0MW > 1) ? 0 : BOOT_0MW;
TEAM_RACE = (TEAM_RACE > 2) ? 0 : TEAM_RACE;
BAUDRATE = (BAUDRATE > 1) ? 0 : BAUDRATE;
}

Expand Down Expand Up @@ -387,7 +384,7 @@ void GetVtxParameter() {
LP_MODE = I2C_Read8(ADDR_EEPROM, EEP_ADDR_LPMODE);
PIT_MODE = I2C_Read8(ADDR_EEPROM, EEP_ADDR_PITMODE);
OFFSET_25MW = I2C_Read8(ADDR_EEPROM, EEP_ADDR_25MW);
BOOT_0MW = I2C_Read8(ADDR_EEPROM, EEP_ADDR_BOOT_0MW);
TEAM_RACE = I2C_Read8(ADDR_EEPROM, EEP_ADDR_TEAM_RACE);
BAUDRATE = I2C_Read8(ADDR_EEPROM, EEP_ADDR_BAUDRATE);

CFG_Back();
Expand Down Expand Up @@ -1460,3 +1457,15 @@ void uart_baudrate_detect(void) {
Setting_Save();
}
}

void vtx_paralized(void) {
// Sleep until repower
WriteReg(0, 0x8F, 0x00);
#ifdef _DEBUG_MODE
debugf("\r\nvtx paralized");
#endif
while (1) {
LED_Flip();
WAIT(50);
}
}
9 changes: 4 additions & 5 deletions src/hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ typedef enum {
#define EEP_ADDR_LPMODE 0x82
#define EEP_ADDR_PITMODE 0x83
#define EEP_ADDR_25MW 0x84
#define EEP_ADDR_BOOT_0MW 0x85
#define EEP_ADDR_TEAM_RACE 0x85
#define EEP_ADDR_SA_LOCK 0x88
#define EEP_ADDR_POWER_LOCK 0x89
#define EEP_ADDR_VTX_CONFIG 0x8A
Expand Down Expand Up @@ -87,10 +87,12 @@ void Set_540P60(uint8_t page);
void Set_1080P30(uint8_t page);

void Flicker_LED(uint8_t n);
void LED_Flip();
void LED_Task();

uint8_t RF_BW_check(void);
void uart_baudrate_detect(void);
void vtx_paralized(void);

#ifdef HDZERO_FREESTYLE
extern uint8_t powerLock;
Expand All @@ -100,7 +102,7 @@ extern uint8_t RF_POWER;
extern uint8_t LP_MODE;
extern uint8_t PIT_MODE;
extern uint8_t OFFSET_25MW;
extern uint8_t BOOT_0MW;
extern uint8_t TEAM_RACE;
extern uint8_t KEYBOARD_ON;
extern uint8_t EE_VALID;
extern uint8_t RF_BW;
Expand All @@ -110,9 +112,6 @@ extern uint8_t BAUDRATE;
extern uint8_t pwr_offset;
extern uint8_t heat_protect;

extern uint8_t g_IS_ARMED;
extern uint8_t g_IS_ARMED_last;
extern uint8_t g_IS_PARALYZE;
extern uint8_t fc_lock;
extern uint8_t vtx_pit;
extern uint8_t vtx_pit_save;
Expand Down
2 changes: 2 additions & 0 deletions src/mcu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,8 @@ void RF_Delay_Init() {
#ifdef _DEBUG_MODE
debugf("\r\nRF_Delay_Init: None");
#endif
if (TEAM_RACE == 0x01)
vtx_paralized();
#if (0)
if (PIT_MODE == PIT_0MW) {
pwr_lmt_done = 1;
Expand Down
77 changes: 48 additions & 29 deletions src/msp_displayport.c
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#include "msp_displayport.h"

#include "camera.h"
#include "common.h"
#include "dm6300.h"
Expand Down Expand Up @@ -32,15 +31,14 @@ uint8_t resolution = SD_3016;
uint8_t resolution_last = HD_5018;

uint8_t msp_rx_buf[64]; // from FC responding status|variant|rc commands
uint8_t disarmed = 1;

uint8_t vtx_channel;
uint8_t vtx_power;
uint8_t vtx_lp;
uint8_t vtx_pit;
uint8_t vtx_pit_save = PIT_OFF;
uint8_t vtx_offset = 0;
uint8_t vtx_boot_0mw = 0;
uint8_t vtx_team_race = 0;
uint8_t first_arm = 0;

uint8_t fc_band_rx = 0;
Expand All @@ -49,9 +47,11 @@ uint8_t fc_pwr_rx = 0;
uint8_t fc_pit_rx = 0;
uint8_t fc_lp_rx = 0;

uint8_t g_IS_ARMED = 0;
uint8_t g_IS_PARALYZE = 0;

uint8_t pit_mode_cfg_done = 0;
uint8_t lp_mode_cfg_done = 0;
uint8_t g_IS_ARMED_last = 0;

uint8_t lq_cnt = 0;

Expand Down Expand Up @@ -88,6 +88,7 @@ uint8_t crc8tab[256] = {

uint8_t osd_menu_offset = 0;
uint32_t msp_lst_rcv_sec = 0;
uint32_t fc_lst_rcv_sec = 0;

uint8_t boot_0mw_done = 0;

Expand Down Expand Up @@ -177,6 +178,7 @@ void msp_task() {

// send param to FC -- 8HZ
// send param to VRX -- 8HZ
// detect fc lost
if (timer_8hz) {
len = get_tx_data_5680();
insert_tx_buf(len);
Expand All @@ -187,6 +189,11 @@ void msp_task() {
msp_tx_cnt++;
else
msp_cmd_tx();

if (seconds - fc_lst_rcv_sec > 2) {
if (TEAM_RACE == 0x01)
vtx_paralized();
}
}

// set_vtx
Expand All @@ -211,6 +218,9 @@ uint8_t msp_read_one_frame() {
return ret;
rx = CMS_rx();

if (TEAM_RACE)
fc_lst_rcv_sec = seconds;

switch (state) {
case MSP_HEADER_START:
if (rx == MSP_HEADER_START_BYTE) {
Expand Down Expand Up @@ -783,13 +793,22 @@ void msp_set_vtx_config(uint8_t power, uint8_t save) {
}

void parse_status() {

if (!(fc_lock & FC_STATUS_LOCK))
fc_lock |= FC_STATUS_LOCK;

g_IS_ARMED = (msp_rx_buf[6] & 0x01);
#if (0)
g_IS_PARALYZE = (msp_rx_buf[9] & 0x80);
disarmed = !g_IS_ARMED;
// debugf("\n\rstatus:%x",msp_rx_buf[6] & 0x01);

if (g_IS_PARALYZE) {
vtx_paralized();
}
#endif

#if (0)
debugf("\n\rstatus:%x %x %x %x", (uint16_t)msp_rx_buf[6], (uint16_t)msp_rx_buf[7], (uint16_t)msp_rx_buf[8], (uint16_t)msp_rx_buf[9]);
#endif
}

void parse_variant() {
Expand All @@ -804,7 +823,6 @@ void parse_variant() {

void parse_rc() {
uint16_t roll, pitch, yaw, throttle;
// static uint16_t roll_d,pitch_d,yaw_d,throttle_d;

if (!(fc_lock & FC_RC_LOCK))
fc_lock |= FC_RC_LOCK;
Expand All @@ -815,12 +833,6 @@ void parse_rc() {
throttle = (msp_rx_buf[7] << 8) | msp_rx_buf[6];

update_cms_menu(roll, pitch, yaw, throttle);
/*
roll_d = roll;
pitch_d = pitch;
yaw_d = yaw;
throttle_d = throttle;
*/
}
void parse_vtx_config() {
uint8_t nxt_ch = 0;
Expand Down Expand Up @@ -938,7 +950,7 @@ void parseMspVtx_V2(uint16_t cmd_u16) {
// update pit
nxt_pwr = fc_pwr_rx - 1;

if ((boot_0mw_done == 0) && BOOT_0MW) {
if ((boot_0mw_done == 0) && TEAM_RACE) {
msp_set_vtx_config(POWER_MAX + 1, 0);
// sometimes FC delay to receive 0mW, so check fc reply power and resend 0mW.
if (nxt_pwr == POWER_MAX + 1) {
Expand Down Expand Up @@ -1170,7 +1182,7 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
uint8_t IS_LO_roll = IS_LO(roll);
uint8_t IS_MID_roll = IS_MID(roll);

if (!disarmed && (cms_state != CMS_OSD)) {
if (g_IS_ARMED && (cms_state != CMS_OSD)) {
fc_init();
cms_state = CMS_OSD;
}
Expand Down Expand Up @@ -1199,7 +1211,7 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt

switch (cms_state) {
case CMS_OSD:
if (disarmed) {
if (!g_IS_ARMED) {
if (IS_HI_yaw && IS_LO_throttle && IS_LO_roll && IS_LO_pitch) {
if (cur_pwr == POWER_MAX + 2) {
cms_state = CMS_EXIT_0MW;
Expand Down Expand Up @@ -1260,7 +1272,7 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
vtx_lp = LP_MODE;
vtx_pit = PIT_OFF;
vtx_offset = OFFSET_25MW;
vtx_boot_0mw = BOOT_0MW;
vtx_team_race = TEAM_RACE;
if (SA_lock) {
// PIT_MODE = 2;
vtx_pit = PIT_P1MW;
Expand Down Expand Up @@ -1296,7 +1308,7 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
}

case CMS_VTX_MENU: {
if (disarmed) {
if (!g_IS_ARMED) {
if (last_mid) {
switch (vtx_state) {
// channel
Expand Down Expand Up @@ -1431,8 +1443,14 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
vtx_state = 6;
else if (VirtualBtn == BTN_UP)
vtx_state = 4;
else if (VirtualBtn == BTN_LEFT || VirtualBtn == BTN_RIGHT) {
vtx_boot_0mw = 1 - vtx_boot_0mw;
else if (VirtualBtn == BTN_LEFT) {
vtx_team_race--;
if (vtx_team_race > 2)
vtx_team_race = 2;
} else if (VirtualBtn == BTN_RIGHT) {
vtx_team_race++;
if (vtx_team_race > 2)
vtx_team_race = 0;
}
update_vtx_menu_param(vtx_state);
break;
Expand Down Expand Up @@ -1553,7 +1571,7 @@ void vtx_menu_init() {
strcpy(osd_buf[4] + osd_menu_offset + 2, " LP_MODE");
strcpy(osd_buf[5] + osd_menu_offset + 2, " PIT_MODE");
strcpy(osd_buf[6] + osd_menu_offset + 2, " OFFSET_25MW");
strcpy(osd_buf[7] + osd_menu_offset + 2, " 0MW BOOT");
strcpy(osd_buf[7] + osd_menu_offset + 2, " TEAM_RACE");
strcpy(osd_buf[8] + osd_menu_offset + 2, " EXIT ");
strcpy(osd_buf[9] + osd_menu_offset + 2, " SAVE&EXIT");
strcpy(osd_buf[10] + osd_menu_offset + 2, "------INFO------");
Expand All @@ -1577,7 +1595,7 @@ void vtx_menu_init() {
vtx_lp = LP_MODE;
vtx_pit = PIT_MODE;
vtx_offset = OFFSET_25MW;
vtx_boot_0mw = BOOT_0MW;
vtx_team_race = TEAM_RACE;
update_vtx_menu_param(0);
}

Expand All @@ -1588,7 +1606,7 @@ void update_vtx_menu_param(uint8_t vtx_state) {
const char *powerString[] = {" 25", " 200", " 500", " MAX"};
const char *lowPowerString[] = {" OFF", " ON", " 1ST"};
const char *pitString[] = {" OFF", " P1MW", " 0MW"};
const char *boot0mwString[] = {" OFF", " ON"};
const char *treamRaceString[] = {" OFF", "MODE1", "MODE2"};

// cursor
vtx_state += 2;
Expand Down Expand Up @@ -1626,7 +1644,7 @@ void update_vtx_menu_param(uint8_t vtx_state) {
} else if (vtx_offset == 20)
strcpy(osd_buf[6] + osd_menu_offset + 20, " -10");

strcpy(osd_buf[7] + osd_menu_offset + 20, boot0mwString[vtx_boot_0mw]);
strcpy(osd_buf[7] + osd_menu_offset + 20, treamRaceString[vtx_team_race]);

ParseLifeTime(hourString, minuteString);
osd_buf[13][osd_menu_offset + 16] = hourString[0];
Expand All @@ -1646,7 +1664,7 @@ void save_vtx_param() {
PIT_MODE = vtx_pit;
vtx_pit_save = vtx_pit;
OFFSET_25MW = vtx_offset;
BOOT_0MW = vtx_boot_0mw;
TEAM_RACE = vtx_team_race;
CFG_Back();
Setting_Save();
Imp_RF_Param();
Expand All @@ -1656,7 +1674,7 @@ void save_vtx_param() {
lp_mode_cfg_done = 0;
first_arm = 0;

if (BOOT_0MW)
if (TEAM_RACE)
boot_0mw_done = 1;

if (!SA_lock) {
Expand All @@ -1668,6 +1686,7 @@ void save_vtx_param() {
}

void set_vtx_param() {
static uint8_t g_IS_ARMED_last = 0;
// If fc is lost, auto armed
/*
if(seconds >= PWR_LMT_SEC){
Expand Down Expand Up @@ -1723,7 +1742,7 @@ void set_vtx_param() {
if (g_IS_ARMED && !g_IS_ARMED_last) {
// Power_Auto
if (vtx_pit_save == PIT_0MW) {
if (BOOT_0MW)
if (TEAM_RACE)
;
else {
// exit 0mW
Expand Down Expand Up @@ -1767,7 +1786,7 @@ void set_vtx_param() {
first_arm = 1;
PIT_MODE = PIT_OFF;
Setting_Save();
if (!BOOT_0MW)
if (!TEAM_RACE)
msp_set_vtx_config(RF_POWER, 1);
} else if (!g_IS_ARMED && g_IS_ARMED_last) {
if (LP_MODE == 1) {
Expand Down Expand Up @@ -1817,7 +1836,7 @@ void InitVtxTable() {
#endif

// set band num, channel num and power level number
if (BOOT_0MW)
if (TEAM_RACE)
msp_set_vtx_config(POWER_MAX + 1, 0);
else
msp_set_vtx_config(fc_pwr_rx, 0);
Expand Down
3 changes: 3 additions & 0 deletions src/msp_displayport.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,4 +169,7 @@ extern uint8_t resolution;
extern uint8_t first_arm;
extern uint8_t mspVtxLock;
extern uint32_t msp_lst_rcv_sec;
extern uint32_t fc_lst_rcv_sec;
extern uint8_t g_IS_ARMED;
extern uint8_t g_IS_PARALYZE;
#endif /* __MSP_DISPLAYPORT_H_ */

0 comments on commit a5e769d

Please sign in to comment.