minisky_multicopter¶

def create(self, n: int = 1) -> None:
    """Seed multicopter state for n newly created aircraft.

    Membership follows from the typecode; the body heading starts
    unconstrained (nose follows track) at the default yaw rate.
    """
    super().create(n)
    self.ismulticopter[-n:] = [
        typecode.upper() in self.typespecs for typecode in self.traffic.typecode[-n:]
    ]
    self.selhdg[-n:] = self.traffic.hdg[-n:]
    self.swselhdg[-n:] = False
    self.yawrate[-n:] = DEFAULT_YAWRATE

def mask(self) -> np.ndarray:
    """Return the boolean row mask of aircraft flown as multicopters."""
    return self.ismulticopter

def select_implementations(self) -> None:
    """Swap the multicopter implementations onto the owning traffic.

    Equivalent to issuing `SELECTIMPL <BASE> <IMPL>` for each entry of
    `IMPLEMENTATIONS`; replaces the live instance immediately.
    """
    # NOTE(abraham): plugin loading should eventually register behaviour,
    # not mutate which concrete implementation owns every aircraft.
    # importing multicopter should not have any side effects!
    for basename, implname in IMPLEMENTATIONS:
        result = self.traffic.select_implementation(basename, implname)
        if result.is_err():
            raise RuntimeError(f"MULTICOPTER: {result.err()}")
    self._selected = True

@hook("preupdate")
def ensure_implementations(self) -> None:
    """Select the multicopter implementations on the first step after load.

    Replacements are installed when the plugin loads but can only be
    selected once the plugin is published, so the initial selection
    happens here. A manual `SELECTIMPL` afterwards is respected until
    the next reset.
    """
    if not self._selected:
        self.select_implementations()

@hook("reset")
def reselect_implementations(self) -> None:
    """Re-select the multicopter implementations after a reset.

    A reset reverts every replaceable to its core default; this hook runs
    afterwards and restores the multicopter set.
    """
    self.select_implementations()

@command(name="MCOPT")
def mcopt_status(self, idx: AcId) -> Result[str, str]:
    """Report whether multicopter behavior is enabled for an aircraft."""
    callsign = self.traffic.callsign[idx]
    return Ok(f"MCOPT {callsign}: {'ON' if self.ismulticopter[idx] else 'OFF'}")

@command(name="MCOPT")
def set_mcopt(self, idx: AcId, flag: OnOff) -> Result[str, str]:
    """Enable or disable multicopter behavior for an aircraft."""
    callsign = self.traffic.callsign[idx]
    if flag and self.traffic.typecode[idx].upper() not in self.typespecs:
        return Err(f"MCOPT: {callsign} type is not configured as a multicopter")

    self.ismulticopter[idx] = flag
    # Multicopters fly point-to-point: newly added waypoints default to
    # fly-over (restored to fly-by when switched back off).
    self.traffic.ap.route[idx].swflyby = not flag
    if flag:
        # Start with the nose unconstrained, following the track.
        self.selhdg[idx] = self.traffic.hdg[idx]
        self.swselhdg[idx] = False
    return Ok(f"MCOPT {callsign}: {'ON' if flag else 'OFF'}")

@command
def yaw(
    self, idx: AcId, hdg: TrueHeadingDeg[IsFinite[float]] | MagneticHeadingDeg[IsFinite[float]]
) -> Result[str, str]:
    """Command the body heading (nose direction) of a multicopter.

    The velocity vector keeps following the track command from the FMS
    or conflict resolution, so this rotates the aircraft in place.
    """
    if not self.ismulticopter[idx]:
        callsign = self.traffic.callsign[idx]
        return Err(f"YAW: {callsign} is not a multicopter")

    resolved_hdg = hdg.value
    if isinstance(hdg, MagneticHeadingDeg):
        resolved_hdg += geo.magdec(float(self.traffic.lat[idx]), float(self.traffic.lon[idx]))
    resolved_hdg %= 360.0
    self.selhdg[idx] = resolved_hdg
    self.swselhdg[idx] = True
    return Ok(f"YAW {self.traffic.callsign[idx]}: nose to {resolved_hdg:.0f} deg")

@command(name="YAWRATE")
def yawrate_status(self, idx: AcId) -> Result[str, str]:
    """Report the maximum yaw rate of a multicopter."""
    callsign = self.traffic.callsign[idx]
    return Ok(f"YAWRATE {callsign}: {self.yawrate[idx]:.0f} deg/s")

@command(name="YAWRATE")
def set_yawrate(
    self,
    idx: AcId,
    yawrate: q.YawRateDegPerS[IsFinite[Gt0[float]]],
) -> Result[str, str]:
    """Set the maximum yaw rate of a multicopter."""
    callsign = self.traffic.callsign[idx]
    self.yawrate[idx] = yawrate
    return Ok(f"YAWRATE {callsign}: {yawrate:.0f} deg/s")

@command
def hover(
    self,
    idx: AcId,
    duration: TimeS | None = None,
    alt: StdPressureAltM[IsFinite[float]] | None = None,
) -> Result[str, str]:
    """Hold position, optionally for a fixed time at a given altitude.

    Suspends LNAV/VNAV, commands zero ground speed, and holds the given
    altitude (the current one when omitted) — with an altitude the
    aircraft moves there vertically, at a fixed position. With a
    duration, the route resumes once position and altitude have been
    held that long; without one, the aircraft hovers until LNAV is
    re-engaged. Repeating the command while hovering updates the hold
    time and altitude, and a plain ALT command changes the hover
    altitude as well.
    """
    # Deferred import: the autopilot module imports this one.
    from minisky_multicopter.autopilot import MulticopterAutopilot

    ap = self.traffic.ap
    if not isinstance(ap, MulticopterAutopilot):
        return Err("HOVER: SELECTIMPL AUTOPILOT MULTICOPTERAUTOPILOT first")
    return ap.hover(idx, duration, alt)

@command
def batt(self, idx: AcId) -> Result[str, str]:
    """Report the battery state of charge, power draw and endurance."""
    # Deferred import: the perf module imports this one.
    from minisky_multicopter.perf import MulticopterPerf

    callsign = self.traffic.callsign[idx]
    if not self.ismulticopter[idx]:
        return Err(f"BATT: {callsign} is not a multicopter")
    perf = self.traffic.perf
    if not isinstance(perf, MulticopterPerf):
        return Err("BATT: SELECTIMPL OPENAP MULTICOPTERPERF first")
    return perf.batt(idx)

def create(self, n: int = 1) -> None:
    """Seed the hover state of n newly created aircraft.

    New aircraft start with no hover active; new multicopters get
    fly-over waypoints by default (they fly point-to-point, without a
    turn-anticipation arc).
    """
    super().create(n)
    self.swhover[-n:] = False
    self.resume_airspeed.values[-n:] = 0.0
    self.resume_airspeed.kind[-n:] = AirspeedKind.CAS
    self.resume_lnav[-n:] = False
    self.resume_vnav[-n:] = False
    self.resume_vnav_airspeed[-n:] = False

    # Membership by typecode from the performance table: the Multicopter
    # entity may be created after this autopilot in the traffic tree, so
    # its arrays cannot be relied upon here.
    mc = get_multicopter(self.traffic)
    if mc is None:
        return
    for offset, typecode in enumerate(self.traffic.typecode[-n:], start=-n):
        if typecode.upper() in mc.typespecs:
            self.route[offset].swflyby = False

def update(self) -> None:
    """Run the FMS, then advance any active hovers (vectorized).

    A timed hover counts its hold time down only while the position is
    actually held: stopped, at the selected altitude. A hover ends when
    that timer expires (the saved route state is restored; the selected
    altitude stays at the hover altitude) or when LNAV is re-engaged
    externally (LNAV is then left as commanded).
    """
    super().update()
    mc = get_multicopter(self.traffic)
    if not self.swhover.any() or mc is None:
        return

    traf = self.traffic
    # LNAV was re-engaged externally: cancel those hovers.
    cancel = self.swhover & traf.swlnav
    # Timed hovers holding position and altitude: count the timer down.
    timed = self.swhover & (self.hovertimer.present)
    holding = (
        timed
        & ~traf.swlnav
        & (traf.gs < mc.config.gs_hover)
        & (np.abs(traf.alt - traf.selalt) < mc.config.alt_capture)
    )
    self.hovertimer.values[holding] -= self.simulation.simdt
    expired = holding & (self.hovertimer.values <= 0.0)

    # Restore the saved route state; expiry also re-engages LNAV/VNAV.
    resume = cancel | expired
    traf.selected_airspeed.values[:] = np.where(
        resume, self.resume_airspeed.values, traf.selected_airspeed.values
    )
    traf.selected_airspeed.kind[:] = np.where(
        resume, self.resume_airspeed.kind, traf.selected_airspeed.kind
    ).astype(np.uint8)
    traf.swvnavairspeed = np.where(resume, self.resume_vnav_airspeed, traf.swvnavairspeed)
    traf.swvnav = np.where(cancel, self.resume_vnav, traf.swvnav)
    traf.swlnav = np.where(expired, self.resume_lnav, traf.swlnav)
    traf.swvnav = np.where(expired, self.resume_vnav & self.resume_lnav, traf.swvnav)
    self.swhover = self.swhover & ~resume
    self.hovertimer.clear(resume)

def selhdgcmd(
    self, idx: AcIdSelection, hdg: TrueHeadingDeg | MagneticHeadingDeg
) -> Result[str, str]:
    """Select the autopilot heading; for multicopters, yaw the nose only.

    For multicopter rows the HDG stack command is an alias of `YAW`:
    it rotates the body without touching the track, and LNAV stays
    engaged — the velocity vector keeps following the FMS or conflict
    resolution. Other aircraft keep the stock behaviour.
    """
    mc = get_multicopter(self.traffic)
    if mc is None:
        return super().selhdgcmd(idx, hdg)

    is_multicopter = mc.ismulticopter[idx]
    if not is_multicopter.any():
        return super().selhdgcmd(idx, hdg)

    message = "heading set"
    for acidx in idx[is_multicopter]:
        result = mc.yaw(int(acidx), hdg)
        if isinstance(result, Err):
            return result
        message = result.ok()

    fixed_wing = idx[~is_multicopter]
    if fixed_wing.size:
        return super().selhdgcmd(fixed_wing, hdg)
    return Ok(message)

def hover(
    self,
    idx: AircraftIndex,
    duration: q.DurationS[float] | None = None,
    alt: StdPressureAltM | None = None,
) -> Result[str, str]:
    """Hold position, optionally for a fixed time at a given altitude.

    Backs the `HOVER` stack command declared on the Multicopter entity,
    which delegates here at call time so the command survives the
    autopilot instance being swapped on reset.

    Args:
        duration: Hold time; `None` holds indefinitely.
        alt: Hover altitude; `None` holds the current altitude.
    """
    callsign = self.traffic.callsign[idx]
    mc = get_multicopter(self.traffic)
    if mc is None or not mc.ismulticopter[idx]:
        return Err(f"HOVER: {callsign} is not a multicopter")

    if not self.swhover[idx]:
        # Entering the hover: save the route state to resume later.
        self._suspend_route(idx)
        self.swhover[idx] = True
    if alt is not None:
        result = self.selaltcmd(np.asarray([idx], dtype=int), alt)
        if isinstance(result, Err):
            return result
    if duration is None:
        self.hovertimer.clear(idx)
    else:
        self.hovertimer.set(idx, duration)

    if duration is None:
        return Ok(f"HOVER {callsign}: holding position (resume with LNAV {callsign} ON)")
    return Ok(f"HOVER {callsign}: holding position for {duration:.0f} s")

def create(self, n: int = 1) -> None:
    """Seed the electric state of n newly created aircraft.

    Multicopters start on a full battery with their typecode's pack
    energy, drag area and thrust-to-weight ratio; other rows keep zeros
    (no battery model). Seeded per row rather than per batch, so a swap
    onto a mixed fleet stays correct. Membership comes from the
    performance table because the Multicopter entity may sit after this
    object in the traffic tree.
    """
    super().create(n)
    mc = get_multicopter(self.traffic)
    if mc is None:
        return
    for offset, typecode in enumerate(self.traffic.typecode[-n:], start=-n):
        actype = typecode.upper()
        spec = mc.typespecs.get(actype)
        if spec is None:
            continue
        self.twr[offset] = spec.twr
        self.cds[offset] = spec.cds
        if (energy := spec.battery_energy) is None:
            energy = self._range_derived_wh(
                spec.airframe, spec.cds, spec.twr, mc.config.cruise_speed_fraction
            )
        self.capacity[offset] = q.wh_to_j(energy)
        self.soc[offset] = 1.0

def required_thrust(self) -> q.ForceN[np.ndarray]:
    r"""Return the thrust each aircraft would need as a multicopter [N].

    The thrust vector supports the weight — including any vertical
    acceleration, $m \sqrt{g^2 + a_z^2}$ — while its horizontal
    component overcomes the flat-plate parasite drag of translation,
    $\tfrac{1}{2} \rho v^2 C_D S$. Meaningful for multicopter rows
    (other rows have a zero drag area).
    """
    traf = self.traffic
    rho = aero.vdensity(traf.alt)
    drag = 0.5 * rho * traf.tas**2 * self.cds
    lift = self.mass * np.hypot(aero.g0, traf.kinematics.az)
    return np.hypot(lift, drag)

def update(self) -> None:
    r"""Update performance, then the electric model for multicopter rows.

    After the base update, computes each multicopter's required thrust,
    derives the electrical power from the momentum-theory scaling
    $P = P_\text{max} (T / T_\text{max})^{1.5}$, and integrates the
    battery state of charge as an ideal energy tank.
    """
    # NOTE(abraham): OpenAP updates the shared performance arrays for the
    # entire fleet first, after which this subclass overwrites multicopter
    # rows!
    super().update()
    mc = get_multicopter(self.traffic)
    if mc is None:
        return
    m = mc.ismulticopter & (self.capacity > 0.0)
    if not m.any():
        return

    thrust = self.required_thrust()[m]
    t_max = self.twr[m] * self.mass[m] * aero.g0
    p_max = self.engnum[m] * self.engpower[m]
    power = p_max * np.clip(thrust / t_max, 0.0, 1.0) ** 1.5

    self.thrust[m] = thrust
    self.power[m] = power
    simdt = self.traffic._get_simulation().simdt
    self.soc[m] = np.clip(self.soc[m] - power * simdt / self.capacity[m], 0.0, 1.0)

def limits(
    self,
    intent_v_tas: q.TrueAirspeedMps[np.ndarray],
    intent_vs: q.VerticalRateMps[np.ndarray],
    intent_h: q.PressureAltitudeM[np.ndarray],
    ax: q.AccelerationMps2[np.ndarray],
) -> OpenAP.PerformanceLimits:
    """Clip the intended state to the flight envelope.

    Runs the base envelope, then tightens the maximum speed and climb
    rate of multicopter rows below the state-of-charge threshold (the
    `soc_low` / `lowbatt_*_factor` plugin settings). Descent stays
    unrestricted — a low battery should not keep an aircraft airborne.
    """
    allowed = super().limits(intent_v_tas, intent_vs, intent_h, ax)
    mc = get_multicopter(self.traffic)
    if mc is None:
        return allowed
    low = mc.ismulticopter & (self.capacity > 0.0) & (self.soc < mc.config.soc_low)
    if not low.any():
        return allowed

    tas, vs, alt = allowed
    tas[low] = np.minimum(tas[low], mc.config.lowbatt_spd_factor * self.vmax[low])
    vs[low] = np.minimum(vs[low], mc.config.lowbatt_vs_factor * self.vsmax[low])
    return self.PerformanceLimits(tas, vs, alt)

def batt(self, idx: AircraftIndex) -> Result[str, str]:
    """Report battery state of charge, power draw and endurance.

    Backs the `BATT` stack command declared on the Multicopter entity,
    which delegates here at call time so the command survives the
    performance instance being swapped on reset.
    """
    callsign = self.traffic.callsign[idx]
    soc = self.soc[idx]
    power = self.power[idx]
    if soc <= 0.0:
        endurance = "battery empty"
    elif power > 0.0:
        endurance = f"endurance {q.s_to_min(soc * self.capacity[idx] / power):.0f} min"
    else:
        endurance = "endurance --"
    return Ok(f"BATT {callsign}: {soc:.0%}, drawing {power:.0f} W, {endurance}")