PilotVault/Fly App/lib/fly/gimbal_control.dart

import 'dart:math' as math;
import 'dart:ui' show Offset;

import 'product_profile.dart';
import 'state/aircraft_state.dart';
import 'state/gimbal_state.dart';

/// Pitch / yaw speed in °/s for `Gimbal.rotate` in speed mode; [yaw] is null
/// when the gimbal does not pan, so that axis is left alone.
class GimbalSpeed {
  const GimbalSpeed(this.pitch, this.yaw);

  static const GimbalSpeed zero = GimbalSpeed(0, 0);

  final double pitch;
  final double? yaw;

  bool get isZero => pitch == 0 && (yaw ?? 0) == 0;

  @override
  bool operator ==(Object other) => other is GimbalSpeed && other.pitch == pitch && other.yaw == yaw;

  @override
  int get hashCode => Object.hash(pitch, yaw);

  @override
  String toString() => 'GimbalSpeed(${pitch.toStringAsFixed(1)}, ${yaw?.toStringAsFixed(1)})';
}

/// Press-and-drag on the live view as a gimbal joystick (GO 4
/// `fpv_gimbal_control`, plan P5): the press point is the centre, and how far
/// the finger has moved from it sets the speed — up tilts the camera up, right
/// pans right. A small dead zone keeps a resting finger still, and the curve is
/// squared so short drags give fine control.
abstract final class GimbalJoystick {
  /// Finger travel (logical px) for full speed.
  static const double reach = 120;

  /// Finger travel that still means "hold".
  static const double deadZone = 8;

  /// Full-deflection speed, °/s.
  static const double maxSpeed = 30;

  /// The speed for a finger [delta] away from the press point.
  static GimbalSpeed speedFor(Offset delta, {required bool yaw}) =>
      GimbalSpeed(_axis(-delta.dy), yaw ? _axis(delta.dx) : null);

  static double _axis(double d) {
    final double mag = d.abs();
    if (mag <= deadZone) return 0;
    final double t = ((mag - deadZone) / (reach - deadZone)).clamp(0.0, 1.0);
    final double v = maxSpeed * t * t;
    return d < 0 ? -v : v;
  }

  /// The finger offset clamped to the joystick ring, for drawing the knob.
  static Offset knob(Offset delta) {
    final double d = delta.distance;
    return d <= reach ? delta : delta * (reach / d);
  }
}

/// A gimbal mode the pilot can pick.
class GimbalModeOption {
  const GimbalModeOption(this.value, this.label, this.hint);
  final String value;
  final String label;
  final String hint;
}

abstract final class GimbalModes {
  static const GimbalModeOption follow =
      GimbalModeOption('YAW_FOLLOW', 'Follow', 'Horizon stays level; the camera turns with the aircraft.');
  static const GimbalModeOption fpv =
      GimbalModeOption('FPV', 'FPV', 'The camera rolls with the aircraft, like a cockpit view.');
  static const GimbalModeOption free =
      GimbalModeOption('FREE', 'Free', 'The camera holds its heading while the aircraft turns.');

  /// Follow and FPV on every aircraft gimbal; Free only where the gimbal pans
  /// on its own (Inspire-class), since it needs an independent yaw axis.
  static List<GimbalModeOption> optionsFor(GimbalProfile? g) =>
      <GimbalModeOption>[follow, fpv, if (g?.yawAdjustable == true) free];

  static String label(String? mode) => switch (mode) {
        'YAW_FOLLOW' => follow.label,
        'FPV' => fpv.label,
        'FREE' => free.label,
        _ => '—',
      };
}

/// One controller tuning value of the gimbal sheet's Advanced section,
/// bound to a `gimbalSettings` id and the profile's range for it.
class GimbalTuning {
  const GimbalTuning(this.id, this.label);
  final String id;
  final String label;

  /// Steps of about a twentieth of the range, so − / + gets across it quickly.
  static int stepFor(AxisRange r) => math.max(1, ((r.max - r.min) / 20).round());

  static const List<GimbalTuning> all = <GimbalTuning>[
    GimbalTuning('pitchMaxSpeed', 'Max pitch speed'),
    GimbalTuning('pitchSmoothing', 'Pitch smoothness'),
    GimbalTuning('pitchSpeedCoefficient', 'Pitch stick sensitivity'),
    GimbalTuning('yawMaxSpeed', 'Max yaw speed'),
    GimbalTuning('yawSmoothing', 'Yaw smoothness'),
    GimbalTuning('yawSpeedCoefficient', 'Yaw stick sensitivity'),
  ];

  /// The rows this gimbal has a range for; all of them as a preview when
  /// nothing is connected.
  static List<(GimbalTuning, AxisRange)> rowsFor(ProductProfile p) => <(GimbalTuning, AxisRange)>[
        for (final GimbalTuning t in all)
          if (!p.connected)
            (t, const AxisRange(0, 100))
          else if (p.gimbal?.controller[t.id] case final AxisRange r)
            (t, r),
      ];
}

/// GO 4's gimbal movement presets: one tap sets max speed and smoothness on
/// both axes, as fractions of each value's range on this gimbal (MSDK V4 has
/// no preset setter for aircraft gimbals, only the per-value ones).
enum GimbalPreset {
  fast('Fast', 0.8, 0.2),
  medium('Medium', 0.5, 0.5),
  slow('Slow', 0.2, 0.8);

  const GimbalPreset(this.label, this.speed, this.smoothing);
  final String label;

  /// Fraction of the max-speed range.
  final double speed;

  /// Fraction of the smoothing range.
  final double smoothing;

  static const List<String> speedIds = <String>['pitchMaxSpeed', 'yawMaxSpeed'];
  static const List<String> smoothingIds = <String>['pitchSmoothing', 'yawSmoothing'];

  /// The values this preset writes, for the tuning ids [ranges] covers.
  Map<String, int> valuesFor(Map<String, AxisRange> ranges) => <String, int>{
        for (final String id in speedIds)
          if (ranges[id] case final AxisRange r) id: (r.min + (r.max - r.min) * speed).round(),
        for (final String id in smoothingIds)
          if (ranges[id] case final AxisRange r) id: (r.min + (r.max - r.min) * smoothing).round(),
      };

  /// The preset the current [settings] match (within a step), or null for a custom mix.
  static GimbalPreset? matching(Map<String, AxisRange> ranges, Object? Function(String id) setting) {
    for (final GimbalPreset p in values) {
      final Map<String, int> want = p.valuesFor(ranges);
      if (want.isEmpty) return null;
      final bool all = want.entries.every((MapEntry<String, int> e) {
        final Object? v = setting(e.key);
        return v is num && (v - e.value).abs() <= GimbalTuning.stepFor(ranges[e.key]!);
      });
      if (all) return p;
    }
    return null;
  }
}

/// The one-tap gimbal actions of the sheet and when they are refused.
abstract final class GimbalActions {
  /// Fine-tune nudge per tap, degrees (GO 4's Adjust Gimbal step).
  static const double fineTuneStep = 0.1;

  /// The pitch for "face down": straight down, or as far as this gimbal goes.
  static double faceDownPitch(AxisRange? pitch) => math.max(-90, pitch?.min ?? -90);

  /// Auto-calibration needs the aircraft still on the ground with the motors
  /// off, and is pointless while one is already running.
  static String? calibrationBlocked(AircraftState a, GimbalState g) {
    if (a.isFlying == true || a.motorsOn == true) return 'Land and stop the motors first';
    if (g.calibrating) return 'Calibrating…';
    return null;
  }
}