Hi Gautier i just did this and it works fine in the
simulator.
The way the function works now allows two modes.
If you give the 2 waypoints needed for the function it
works like the original intended and exits.
If you specify any waypoint as 0 then the airplane goes up
for the time specified (usually 3,5 seconds) and then
the function exits.
I also added a waypoin altitude safety check near the end
of the function.
Here is the new function:
bool_t nav_catapult_run(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));
if (_to != 0 && _climb != 0){
nav_catapult_x = stateGetPositionEnu_f()->x;
nav_catapult_y = stateGetPositionEnu_f()->y;
// Store take-off waypoint
WaypointX(_to) = nav_catapult_x;
WaypointY(_to) = nav_catapult_y;
WaypointAlt(_to) = stateGetPositionUtm_f()->alt;
}
}
// 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) {
if (_to != 0 && _climb != 0){
if (NavApproachingFrom(_climb,_to,CARROT)){
nav_catapult_armed = 0;
return FALSE;
}else{
NavSegment(_to, _climb);
NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
NavVerticalAltitudeMode(alt, 0.);
}
}else{ nav_catapult_armed = 0; return FALSE; }
} else {
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
if (_to != 0 && _climb != 0){
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) *
200; // was 300
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) *
200; // was 300
if (alt < (ground_alt+30)){ alt = (ground_alt+30); }
// for added safety
DownlinkSendWp(&(DefaultChannel).trans_tx,
&(DefaultDevice).device, _climb);
}
}
return TRUE;
}