void Takeoff::on_activation() { /* set current mission item to Takeoff */ set_takeoff_item(&_mission_item, _param_min_alt.get()); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw; pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(true); _navigator->set_position_setpoint_triplet_updated(); }
void Takeoff::set_takeoff_position() { struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); float abs_altitude = 0.0f; float min_abs_altitude; if (_navigator->home_position_valid()) { //only use home position if it is valid min_abs_altitude = _navigator->get_global_position()->alt + _param_min_alt.get(); } else { //e.g. flow min_abs_altitude = _param_min_alt.get(); } // Use altitude if it has been set. If home position is invalid use min_abs_altitude if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { abs_altitude = rep->current.alt; // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. if (abs_altitude < min_abs_altitude) { abs_altitude = min_abs_altitude; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); } } else { // Use home + minimum clearance but only notify. abs_altitude = min_abs_altitude; mavlink_log_info(_navigator->get_mavlink_log_pub(), "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); } if (abs_altitude < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. abs_altitude = _navigator->get_global_position()->alt; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); } // set current mission item to takeoff set_takeoff_item(&_mission_item, abs_altitude); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; if (rep->current.valid) { // Go on and check which changes had been requested if (PX4_ISFINITE(rep->current.yaw)) { pos_sp_triplet->current.yaw = rep->current.yaw; } if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) { pos_sp_triplet->current.lat = rep->current.lat; pos_sp_triplet->current.lon = rep->current.lon; } // mark this as done memset(rep, 0, sizeof(*rep)); } _navigator->set_can_loiter_at_sp(true); _navigator->set_position_setpoint_triplet_updated(); }