bool_t nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = GetPosX();
WaypointY(_td) = GetPosY();
WaypointAlt(_td) = GetPosAlt();
return FALSE;
}
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase
if (nav_catapult_launch <= nav_catapult_motor_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(0));
// Store take-off waypoint
WaypointX(_to) = GetPosX();
WaypointY(_to) = GetPosY();
WaypointAlt(_to) = GetPosAlt();
nav_catapult_x = stateGetPositionEnu_f()->x;
nav_catapult_y = stateGetPositionEnu_f()->y;
}
// No Roll, Climb Pitch, Full Power
else if (nav_catapult_launch < nav_catapult_heading_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
}
// Normal Climb: Heading Locked by Waypoint Target
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
} // end of gls()
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase 零滚转 府仰爬行 没有电机阶段
if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(0)); //设定油门
// Store take-off waypoint 存储起飞点
WaypointX(_to) = GetPosX(); //获得ｘ坐标
WaypointY(_to) = GetPosY(); //获得ｙ坐标
WaypointAlt(_to) = GetPosAlt(); //获得高度
nav_catapult_x = stateGetPositionEnu_f()->x; //起飞点ｘ坐标
nav_catapult_y = stateGetPositionEnu_f()->y; //起飞点ｙ坐标
}
// No Roll, Climb Pitch, Full Power 零滚转 府仰爬行 满油门
else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); //设定油门
}
// Normal Climb: Heading Locked by Waypoint Target
// 正常爬行：锁定给定航点
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope) 水平模式（跟随滑坡）
NavVerticalAutoThrottleMode(0); // throttle mode 油门模式
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser) 垂直模式（保持定位）
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
}
void max7456_periodic(void) {
float temp = 0;
//This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
//If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
if (max7456_osd_status == OSD_IDLE) {
if(osd_enable > 1)
osd_enable = 1;
if ((osd_enable<<3) != osd_enable_val) {
osd_enable_val = (osd_enable<<3);
max7456_osd_status = OSD_UNINIT;
}
}
//INITIALIZATION OF THE OSD
if (max7456_osd_status == OSD_UNINIT) {
step = 0;
max7456_trans.status = SPITransDone;
max7456_trans.output_buf[0] = OSD_VM0_REG;
//This operation needs at least 100us but when the periodic function will be invoked again
//sufficient time will have elapsed even with at a periodic frequency of 1000 Hz
max7456_trans.output_buf[1] = OSD_RESET;
max7456_osd_status = OSD_INIT1;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_INIT2) {
max7456_trans.output_length = 1;
max7456_trans.input_length = 1;
max7456_trans.output_buf[0] = OSD_OSDBL_REG_R;
max7456_osd_status = OSD_INIT3;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN
//draw_osd();
switch (step) {
case (0):
osd_put_s("HDG", FALSE, 3, 0, 13);
step = 10;
break;
case (10):
temp = ((float)electrical.vsupply)/10;
osd_sprintf(osd_string, "%.1fV", temp );
if (temp > LOW_BAT_LEVEL)
osd_put_s(osd_string, FALSE, 8, 0, 2);
else
osd_put_s(osd_string, BLINK|INVERT, 8, 0, 2);
step = 20;
break;
case (20):
#if MAG_HEADING_AVAILABLE && !defined(SITL)
temp = DegOfRad(MAG_Heading);
if (temp < 0)
temp += 360;
#else
temp = DegOfRad(state.h_speed_dir_f);
if (temp < 0)
temp += 360;
#endif
osd_sprintf(osd_string, "%.0f", temp);
osd_put_s(osd_string, FALSE, 8, 1, 13);
step = 30;
break;
case (30):
osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f*3.6));
osd_put_s(osd_string, FALSE, 8, 0, 24);
step = 40;
break;
case (40):
osd_sprintf(osd_string, "%.0fm", GetPosAlt() );
osd_put_s(osd_string, FALSE, 10, 13, 2);
step = 50;
break;
case (50):
osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
osd_put_s(osd_string, FALSE, 7, 13, 24);
step = 10;
break;
default: break;
}
}
return;
}
