diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index 37102cebf0e..845415b6d91 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -160,6 +160,9 @@ this reason ensure that you define enough ranges to cover the range channel's us | 56 | NAV_FW_CONTROL_SMOOTHNESS | | 57 | FW_TPA_TIME_CONSTANT | | 58 | FW_LEVEL_TRIM | +| 59 | NAV_WP_MULTI_MISSION_INDEX | +| 60 | NAV_FW_ALT_CONTROL_RESPONSE | +| 61 | MZTC_ZOOM | ## Examples diff --git a/docs/MassZero_Thermal_Camera.md b/docs/MassZero_Thermal_Camera.md new file mode 100644 index 00000000000..15fbd903c2c --- /dev/null +++ b/docs/MassZero_Thermal_Camera.md @@ -0,0 +1,333 @@ +# MassZero Thermal Camera Integration + +## Overview + +The MassZero Thermal Camera (MZTC) integration lets INAV configure and control a MassZero thermal camera over a UART. INAV sets the camera's image parameters, palette, zoom, mirroring and shutter behaviour. It surfaces the link state on the OSD and over MSP. + +The camera produces its own analog video. That video goes to the video transmitter in the usual way. The camera exposes a UART for control and a composite video output for the picture. It has no digital data interface. Its serial protocol has no frame-read command. INAV therefore has no access to thermal pixels or per-pixel temperatures. The OSD elements described here report camera state. + +## Purchase information + +**MassZero Thermal Camera** +Website: [https://masszerofpv.com](https://masszerofpv.com) +Model: MassZero Thermal Camera Core Component +Contact MassZero for current pricing and availability. + +## Build support + +`USE_MZTC` is enabled on targets with more than 512 KB of flash and on SITL. Targets with 512 KB or less do not include the feature. None of the settings or commands below exist on those boards. + +## Hardware requirements + +- An INAV-compatible flight controller with more than 512 KB of flash +- A MassZero Thermal Camera Core Component +- A free UART +- A power supply matching the camera's specification + +## Wiring + +### Serial connection + +The FC controls the camera over a normal bidirectional UART. + +- **TX (FC)** goes to **RX (camera)** +- **RX (FC)** goes to **TX (camera)** +- **GND** goes to **GND** +- **VCC** goes to the camera's supply pin at the voltage its datasheet specifies + +### Video connection + +The camera's analog video output goes to the video transmitter, or to whatever else consumes composite video on the aircraft. It does not connect to the flight controller. INAV has no analog video input. + +### Complete wiring example + +``` +MassZero Thermal Camera Destination +======================= ================================ +VCC -> FC regulated supply (see datasheet) +GND -> FC GND +TX -> FC RX (UART2) +RX -> FC TX (UART2) +Video Out -> VTX video in +GND (video) -> VTX GND +``` + +## Serial port setup + +Assign the `MZTC` function to the UART the camera is wired to. Then tell the driver which port to use. The port index in `mztc_port` is the same zero-based index the CLI `serial` command uses. UART1 is 0, UART2 is 1, and so on. + +``` +set mztc_enabled = ON +set mztc_port = 1 +set mztc_baudrate = 8 +save +``` + +`mztc_baudrate` is an index into INAV's baud rate table. Index 8 is 115200. MassZero cameras ship at that rate. + +## How the link is established + +The driver does not treat an open UART as a working camera. After it opens the port it stays in the initializing state. It sends a read-model command roughly twice a second. The camera is only reported as connected once it answers with a valid packet. If no valid packet arrives for three seconds the driver closes the port, flags a timeout and starts over. + +`mztc` reporting `Connected: NO` with a rising error flag is a genuine wiring, baud rate or power problem. + +`Link quality` is the share of recent probes the camera answered. A marginal connection shows up as a value below 100. + +## Configuration + +All persistent settings live in the CLI and are documented in [Settings.md](Settings.md). The `mztc_*` commands below act on the camera immediately. They do not survive a reboot on their own. + +### Purpose presets + +A preset is a named bundle of image settings for a task. Selecting one writes +the palette, brightness, contrast, digital enhancement, both denoise levels, +the shutter mode and the correction interval. The camera has no preset mechanism of its own. Every preset is ordinary camera commands sent by the flight controller. + +``` +set mztc_preset = SEARCH +``` + +| Preset | Palette | Bright | Contrast | Enhance | Spatial | Temporal | Shutter | FFC | +| --- | --- | --- | --- | --- | --- | --- | --- | --- | +| `CUSTOM` | writes nothing | | | | | | | | +| `GENERAL` | White Hot | 50 | 50 | 50 | 40 | 20 | time and temp | 5 | +| `FIRE` | Iron Red 1 | 45 | 75 | 25 | 30 | 15 | time and temp | 10 | +| `SEARCH` | White Hot | 55 | 60 | 80 | 20 | 10 | time and temp | 3 | +| `SURVEILLANCE` | Green Hot | 50 | 55 | 60 | 45 | 45 | time and temp | 15 | +| `INSPECTION` | Rainbow | 50 | 45 | 70 | 55 | 35 | time and temp | 2 | +| `MARITIME` | Black Hot | 50 | 70 | 45 | 25 | 15 | time and temp | 5 | + +`CUSTOM` is the default and writes nothing. A hand-tuned configuration stays +intact. Adjusting any setting a preset owns is how you return to `CUSTOM`. + +Every route to the setting applies it. `set mztc_preset = SEARCH`, the +`mztc_preset` CLI command, the configurator dropdown and +`MSP2_SET_MZTC_PRESET` all write the same eight values. A preset selected +with `set` takes effect on the next driver tick. + +A reboot does not reapply. The saved values already reflect the saved +preset. Reapplying at boot would discard any tuning done afterwards. + +Zoom and mirror are never written by a preset. Zoom belongs to the pilot. +Mirror describes how the camera is mounted. + +Two constraints shape the numbers. Temporal denoising averages across frames. On a moving airframe it therefore smears targets and leaves trails. Spatial denoising trades noise for sharpness. A person at search range is only a few pixels wide. Both stay low wherever small distant targets matter. `SURVEILLANCE` is the one preset that raises temporal denoising. Loiter and hover leave little frame to frame motion for it to smear. + +`FIRE` keeps digital enhancement low on purpose. Enhancement lifts mid-tones. Flat mid-tones are what let an extreme hot spike dominate the image. + +`SEARCH` does the opposite. A clothed body sits a few degrees over ambient. Enhancement goes high to lift that small signal. Its short correction interval matters more than it looks. A drifting sensor grows fixed-pattern blobs that read as false targets. + +### Image parameters + +``` +set mztc_brightness = 50 +set mztc_contrast = 50 +set mztc_digital_enhancement = 50 +set mztc_spatial_denoise = 50 +set mztc_temporal_denoise = 50 +``` + +All five accept 0 to 100. The same values can be pushed to the camera without saving: + +``` +mztc_config 60 55 70 +mztc_denoise 40 60 +mztc_enhancement 70 +``` + +### Colour palette + +``` +set mztc_palette_mode = WHITE_HOT +``` + +| Value | Palette | +| --- | --- | +| `WHITE_HOT` | White hot, the default | +| `BLACK_HOT` | Black hot | +| `FUSION_1` | Fusion 1 | +| `RAINBOW` | Rainbow | +| `FUSION_2` | Fusion 2 | +| `IRON_RED_1` | Iron red 1 | +| `IRON_RED_2` | Iron red 2 | +| `SEPIA` | Sepia | +| `COLOR_1` | Colour 1 | +| `COLOR_2` | Colour 2 | +| `ICE_FIRE` | Ice fire | +| `RAIN` | Rain | +| `GREEN_HOT` | Green hot | +| `RED_HOT` | Red hot | + +`mztc_palette 5` applies a palette immediately by index. + +### Zoom and mirror + +``` +set mztc_zoom_level = 1X +set mztc_mirror_mode = NONE +``` + +Zoom accepts `1X`, `2X`, `4X` and `8X`. Mirror accepts `NONE`, `HORIZONTAL`, `VERTICAL` and `CENTRAL`. `mztc_zoom 2` applies a zoom level immediately by index. + +The camera manual contradicts itself on the zoom labels. Its prose says 1x, 2x, 4x and 8x. Its value table for the same command says 1x, 2x, 3x and 4x. The wire values 0 to 3 are the same either way. The setting works regardless of which set of labels is right. + +### Shutter and flat field correction + +A manual shutter cycle on this camera performs a flat field correction. One command covers both: + +``` +mztc_calibrate +``` + +Automatic correction is driven by two settings: + +``` +set mztc_auto_shutter = TIME_AND_TEMP +set mztc_ffc_interval = 5 +``` + +`mztc_auto_shutter` accepts `TEMP_ONLY`, `TIME_ONLY` and `TIME_AND_TEMP`. `mztc_ffc_interval` is in minutes and accepts 1 to 60. + +The camera runs the shutter schedule itself. INAV pushes both values to it on connect and then leaves it alone. Select `TEMP_ONLY` to stop the camera correcting on a timer, since it then reacts to temperature drift only. + +### Vignetting correction + +Vignetting correction is a one-shot action. There is no setting for it: + +``` +mztc_vignetting +``` + +Point the lens at a uniform surface before running it. The camera superimposes whatever it is looking at onto the correction. A cluttered scene makes the image worse. + +Bad pixel removal is not exposed. The camera drives it through an on-screen cursor that has to be walked onto each bad pixel. A flight controller cannot do that usefully. + +## Switches and in-flight adjustment + +Both appear in the configurator's Modes tab automatically. Neither is offered +until the camera has a UART assigned in the Ports tab. + +### THERMAL CALIBRATE + +A box mode that runs one flat field correction on the rising edge of the +switch. Holding the switch does not repeat it. + +This is safe to fire at any time. The correction uses the camera's own internal shutter as its reference. The sensor cannot see the scene while it runs, and where the aircraft is pointing does not matter. The camera already +performs the same correction on its own timer. The only cost is roughly a +second of frozen image. + +### MZTC_ZOOM + +An in-flight adjustment that steps the digital zoom through 1x, 2x, 4x and 8x. +Assign it like any other adjustment, described in +[Inflight Adjustments.md](Inflight%20Adjustments.md). It is function 61. + +The adjustment writes the camera and the stored setting together. The switch position and the saved zoom level therefore still agree after a reconnect. + +### What is deliberately not on a switch + +`mztc_vignetting` stays a CLI command. Vignetting correction has no protective +shutter and captures whatever the lens is pointed at. The camera manual is explicit. The lens must face a uniform surface first. Otherwise the current scene is superimposed on every later image. Running it by accident +from a switch would leave a ghost over the picture. + +It is recoverable. `mztc_defaults` restores the camera and clears it. + +The bad pixel commands and manual background correction are not implemented at +all, for the same reason. + +## CLI commands + +| Command | Purpose | +| --- | --- | +| `mztc` | Print the camera state, link quality and reported device ID | +| `mztc_preset <0-6>` | Apply a purpose preset. With no argument it lists them | +| `mztc_config ` | Set the three image parameters at once | +| `mztc_palette <0-13>` | Set the colour palette | +| `mztc_zoom <0-3>` | Set the digital zoom level | +| `mztc_enhancement <0-100>` | Set digital enhancement on its own | +| `mztc_denoise ` | Set both denoising levels | +| `mztc_calibrate` | Trigger a manual shutter cycle for a flat field correction | +| `mztc_vignetting` | Run one vignetting correction | +| `mztc_save` | Save the current image settings to the camera's own flash | +| `mztc_defaults` | Restore the camera to its factory defaults | +| `mztc_reconnect` | Close the port and restart the connection sequence | + +Every command called with no arguments prints the current value. + +## MSP commands + +The camera is reachable over MSP V2 in INAV's own command range. Full payload layouts are in [the MSP message reference](development/msp/README.md). + +| Command | Code | Direction | Payload | +| --- | --- | --- | --- | +| `MSP2_MZTC_CONFIG` | 0x2240 | Out | 11 bytes | +| `MSP2_MZTC_STATUS` | 0x2241 | Out | 7 bytes | +| `MSP2_SET_MZTC_CONFIG` | 0x2242 | In | 11 bytes | +| `MSP2_SET_MZTC_PRESET` | 0x2243 | In | 1 byte | +| `MSP2_SET_MZTC_PALETTE` | 0x2244 | In | 1 byte | +| `MSP2_SET_MZTC_ZOOM` | 0x2245 | In | 1 byte | +| `MSP2_SET_MZTC_SHUTTER` | 0x2246 | In | 0 or 1 bytes | +| `MSP2_SET_MZTC_IMAGE_PARAMS` | 0x2247 | In | 3 bytes | +| `MSP2_SET_MZTC_CORRECTION` | 0x2248 | In | 2 bytes | +| `MSP2_SET_MZTC_VIGNETTING` | 0x2249 | In | 0 or 1 bytes | + +Every field is read and written individually. The wire layout never depends on compiler padding. + +`MSP2_SET_MZTC_CONFIG` validates the whole request against the same limits the CLI enforces before it applies any field. A request that fails validation is rejected in full. It changes nothing. + +## OSD integration + +The element below is an ordinary INAV OSD item. Position and enable it through the OSD layout in the configurator or with the `osd_layout` CLI command, exactly like any other element. + +| Element | Shows | +| --- | --- | +| `OSD_MZTC_STATUS` | A three letter link state: `OK`, `INI`, `FFC`, `REC`, `ALT`, `ERR` or `OFF`. Blinks on `ERR` and `OFF` | + +## Application setups + +The presets above cover the common tasks. Use them as a starting point, then +adjust anything that does not suit the airframe or the conditions. Any +adjustment moves the selection to `CUSTOM` and nothing is overwritten +afterwards. + +### Rapidly changing conditions + +No preset covers this. It describes a condition, where every preset describes a purpose. When +ambient temperature moves quickly, shorten the correction interval and leave +the shutter on its combined trigger. The cost is more shutter interruptions. + +``` +set mztc_auto_shutter = TIME_AND_TEMP +set mztc_ffc_interval = 1 +save +``` + +### What the camera cannot do + +The camera reports no scene temperature over its serial protocol. There is no +spot reading, no maximum in frame and no threshold alarm. A preset tunes the image for a task. It cannot detect a fire or a person. No setting here turns image tuning into detection. + +## Troubleshooting + +### The camera never connects + +`mztc` shows `Connected: NO` and the error flags include 0x01 or 0x10. + +1. Confirm `mztc_enabled` is `ON` and `mztc_port` names the UART the camera is on. +2. Confirm the `MZTC` serial function is assigned to that UART. +3. Confirm `mztc_baudrate` is 8, unless the camera has been reconfigured away from 115200. +4. Check that TX and RX are crossed. +5. Check the supply voltage against the camera's datasheet. +6. Run `mztc_reconnect` after each change. + +### The camera connects and then drops out + +The driver closes the port after three seconds without a valid reply. Intermittent dropouts usually mean marginal wiring or a supply that sags. `Link quality` in the `mztc` output falls before the link drops. Watch it as an early warning. + +### The image is poor + +Raise `mztc_digital_enhancement` and `mztc_contrast` for a flat scene. Raise `mztc_spatial_denoise` and `mztc_temporal_denoise` for a grainy one. Run `mztc_calibrate` if the whole image has drifted. Shorten `mztc_ffc_interval` if it drifts again quickly. + +## Safety + +A thermal camera adds weight and current draw. Check the all-up weight and the power budget before flying. Local rules on thermal imaging vary. Confirm what applies where you fly. diff --git a/docs/Settings.md b/docs/Settings.md index 91a96d16752..0b6163b4720 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3607,6 +3607,143 @@ Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this r --- +### mztc_auto_shutter + +Auto shutter calibration mode + +| Allowed Values | | +| --- | --- | +| TEMP_ONLY | | +| TIME_ONLY | | +| TIME_AND_TEMP | Default | + +--- + +### mztc_brightness + +Image brightness (0-100) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### mztc_contrast + +Image contrast (0-100) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### mztc_digital_enhancement + +Digital enhancement level (0-100) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### mztc_ffc_interval + +Automatic shutter interval in minutes. The camera runs the schedule itself. Set mztc_auto_shutter to TEMP_ONLY to stop it correcting on a timer. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 1 | 60 | + +--- + +### mztc_mirror_mode + +Image mirroring mode + +| Allowed Values | | +| --- | --- | +| NONE | Default | +| HORIZONTAL | | +| VERTICAL | | +| CENTRAL | | + +--- + +### mztc_palette_mode + +Color palette mode + +| Allowed Values | | +| --- | --- | +| WHITE_HOT | Default | +| BLACK_HOT | | +| FUSION_1 | | +| RAINBOW | | +| FUSION_2 | | +| IRON_RED_1 | | +| IRON_RED_2 | | +| SEPIA | | +| COLOR_1 | | +| COLOR_2 | | +| ICE_FIRE | | +| RAIN | | +| GREEN_HOT | | +| RED_HOT | | + +--- + +### mztc_preset + +Purpose preset for the MassZero Thermal Camera. Selecting one writes the palette, brightness, contrast, enhancement, denoise, shutter mode and correction interval. CUSTOM writes nothing and keeps hand-tuned values + +| Allowed Values | | +| --- | --- | +| CUSTOM | Default | +| GENERAL | | +| FIRE | | +| SEARCH | | +| SURVEILLANCE | | +| INSPECTION | | +| MARITIME | | + +--- + +### mztc_spatial_denoise + +Spatial denoising level (0-100) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### mztc_temporal_denoise + +Temporal denoising level (0-100) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### mztc_zoom_level + +Digital zoom level + +| Allowed Values | | +| --- | --- | +| 1X | Default | +| 2X | | +| 4X | | +| 8X | | + +--- + ### name Craft name diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index f35a79211b4..8dedc58afc0 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -461,6 +461,16 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8744 - MSP2_INAV_TIMESYNC](#msp2_inav_timesync) [8752 - MSP2_INAV_SET_AUX_RC](#msp2_inav_set_aux_rc) [8753 - MSP2_INAV_WIND](#msp2_inav_wind) +[8768 - MSP2_MZTC_CONFIG](#msp2_mztc_config) +[8769 - MSP2_MZTC_STATUS](#msp2_mztc_status) +[8770 - MSP2_SET_MZTC_CONFIG](#msp2_set_mztc_config) +[8771 - MSP2_SET_MZTC_PRESET](#msp2_set_mztc_preset) +[8772 - MSP2_SET_MZTC_PALETTE](#msp2_set_mztc_palette) +[8773 - MSP2_SET_MZTC_ZOOM](#msp2_set_mztc_zoom) +[8774 - MSP2_SET_MZTC_SHUTTER](#msp2_set_mztc_shutter) +[8775 - MSP2_SET_MZTC_IMAGE_PARAMS](#msp2_set_mztc_image_params) +[8776 - MSP2_SET_MZTC_CORRECTION](#msp2_set_mztc_correction) +[8777 - MSP2_SET_MZTC_VIGNETTING](#msp2_set_mztc_vignetting) [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) [12289 - MSP2_RX_BIND](#msp2_rx_bind) @@ -4878,6 +4888,154 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Requires `USE_WIND_ESTIMATOR`; returns zeroes when wind estimation is not compiled in or not yet valid. Check bit 0 of `flags` before using speed/angle values. +## `MSP2_MZTC_CONFIG (8768 / 0x2240)` +**Description:** Reads the MassZero thermal camera configuration. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `preset` | `uint8_t` | 1 | - | Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME. | +| `palette_mode` | `uint8_t` | 1 | - | Colour palette. See `mztcPaletteMode_e`. 0-13. | +| `auto_shutter` | `uint8_t` | 1 | - | Automatic shutter policy. See `mztcShutterMode_e`. 0-2. | +| `digital_enhancement` | `uint8_t` | 1 | % | Digital enhancement level. 0-100. | +| `spatial_denoise` | `uint8_t` | 1 | % | Spatial denoising level. 0-100. | +| `temporal_denoise` | `uint8_t` | 1 | % | Temporal denoising level. 0-100. | +| `brightness` | `uint8_t` | 1 | % | Image brightness. 0-100. | +| `contrast` | `uint8_t` | 1 | % | Image contrast. 0-100. | +| `zoom_level` | `uint8_t` | 1 | - | Digital zoom. See `mztcZoomLevel_e`. 0-3. | +| `mirror_mode` | `uint8_t` | 1 | - | Image mirroring. See `mztcMirrorMode_e`. 0-3. | +| `ffc_interval` | `uint8_t` | 1 | minutes | Automatic shutter interval in minutes. 1-60. The camera runs the schedule itself. | + +**Notes:** Requires `USE_MZTC`. Fixed 12 byte reply. The serial port and its baud rate are not in this payload. They come from the Ports tab. Each field is written with the `sbufWrite*` helpers. The layout never depends on compiler padding. + +## `MSP2_MZTC_STATUS (8769 / 0x2241)` +**Description:** Reads the live state of the MassZero thermal camera link. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `status` | `uint8_t` | 1 | - | Camera state. 0 offline, 1 initializing, 2 ready, 3 capturing, 4 calibrating, 5 error, 6 alert, 7 recording. | +| `preset` | `uint8_t` | 1 | - | Purpose preset currently in effect. See `mztcPreset_e`. | +| `connected` | `uint8_t` | 1 | - | Set once the camera has answered a command. Opening the serial port alone does not set it. | +| `connection_quality` | `uint8_t` | 1 | % | Share of recent identity probes the camera answered. 0-100. | +| `last_calibration` | `uint16_t` | 2 | minutes | Time since the last flat field correction. Saturates at 65535. | +| `error_flags` | `uint8_t` | 1 | - | Bit field. 0x01 communication, 0x02 calibration, 0x04 temperature, 0x08 memory, 0x10 timeout, 0x20 invalid config. | + +**Notes:** Requires `USE_MZTC`. Fixed 7 byte reply. + +## `MSP2_SET_MZTC_CONFIG (8770 / 0x2242)` +**Description:** Writes the MassZero thermal camera configuration. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `preset` | `uint8_t` | 1 | - | Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME. | +| `palette_mode` | `uint8_t` | 1 | - | Colour palette. See `mztcPaletteMode_e`. 0-13. | +| `auto_shutter` | `uint8_t` | 1 | - | Automatic shutter policy. See `mztcShutterMode_e`. 0-2. | +| `digital_enhancement` | `uint8_t` | 1 | % | Digital enhancement level. 0-100. | +| `spatial_denoise` | `uint8_t` | 1 | % | Spatial denoising level. 0-100. | +| `temporal_denoise` | `uint8_t` | 1 | % | Temporal denoising level. 0-100. | +| `brightness` | `uint8_t` | 1 | % | Image brightness. 0-100. | +| `contrast` | `uint8_t` | 1 | % | Image contrast. 0-100. | +| `zoom_level` | `uint8_t` | 1 | - | Digital zoom. See `mztcZoomLevel_e`. 0-3. | +| `mirror_mode` | `uint8_t` | 1 | - | Image mirroring. See `mztcMirrorMode_e`. 0-3. | +| `ffc_interval` | `uint8_t` | 1 | minutes | Automatic shutter interval in minutes. 1-60. The camera runs the schedule itself. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 11 bytes. The serial port and its baud rate are not in this payload. They come from the Ports tab. The whole request is validated against the `MZTC_*` limits before any field is applied. A rejected request leaves the running configuration untouched. + +## `MSP2_SET_MZTC_PRESET (8771 / 0x2243)` +**Description:** Applies a purpose preset to the MassZero thermal camera. A preset writes the palette, brightness, contrast, digital enhancement, both denoise levels, the shutter mode and the correction interval. CUSTOM writes nothing. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `preset` | `uint8_t` | 1 | Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 1 byte. + +## `MSP2_SET_MZTC_PALETTE (8772 / 0x2244)` +**Description:** Sets the MassZero thermal camera colour palette. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `palette` | `uint8_t` | 1 | Colour palette. See `mztcPaletteMode_e`. 0-13. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 1 byte. Returns an error if the camera is not connected. + +## `MSP2_SET_MZTC_ZOOM (8773 / 0x2245)` +**Description:** Sets the MassZero thermal camera digital zoom level. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `zoom_level` | `uint8_t` | 1 | Digital zoom. See `mztcZoomLevel_e`. 0-3. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 1 byte. Returns an error if the camera is not connected. + +## `MSP2_SET_MZTC_SHUTTER (8774 / 0x2246)` +**Description:** Triggers a manual shutter cycle for a flat field correction. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `trigger` | `uint8_t` | 1 | Ignored. The command itself is the trigger. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Accepts 0 or 1 bytes. A manual shutter cycle is the flat field correction on this camera. The `mztc_calibrate` CLI command performs the same operation. + +## `MSP2_SET_MZTC_IMAGE_PARAMS (8775 / 0x2247)` +**Description:** Sets the MassZero thermal camera image parameters. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `brightness` | `uint8_t` | 1 | % | Image brightness. 0-100. | +| `contrast` | `uint8_t` | 1 | % | Image contrast. 0-100. | +| `enhancement` | `uint8_t` | 1 | % | Digital enhancement. 0-100. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 3 bytes. Returns an error if the camera is not connected. + +## `MSP2_SET_MZTC_CORRECTION (8776 / 0x2248)` +**Description:** Sets the MassZero thermal camera denoising parameters. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `spatial_denoise` | `uint8_t` | 1 | % | Spatial denoising. 0-100. | +| `temporal_denoise` | `uint8_t` | 1 | % | Temporal denoising. 0-100. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Expects 2 bytes. Returns an error if the camera is not connected. + +## `MSP2_SET_MZTC_VIGNETTING (8777 / 0x2249)` +**Description:** Runs one vignetting correction on the camera. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `trigger` | `uint8_t` | 1 | Ignored. The command itself is the trigger. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MZTC`. Accepts 0 or 1 bytes. The camera manual requires the lens to be pointed at a uniform surface before this runs, so it is an action and never a stored setting. + ## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 7f5e0f6f1cb..b3bd424f12c 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -3,7 +3,7 @@ "fc_version": { "major": 9, "minor": 1, - "patch": 0 + "patch": 1 } }, "enums": { @@ -133,7 +133,8 @@ "ADJUSTMENT_FW_LEVEL_TRIM": "58", "ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX": "59", "ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE": "60", - "ADJUSTMENT_FUNCTION_COUNT": "61" + "ADJUSTMENT_MZTC_ZOOM": "61", + "ADJUSTMENT_FUNCTION_COUNT": "62" }, "adjustmentMode_e": { "_source": "inav/src/main/fc/rc_adjustments.h", @@ -651,7 +652,9 @@ "BOXAUTOSPEED": "60", "BOXTERRAINAGLHOLD": "61", "BOXINFLIGHTMENU": "62", - "CHECKBOX_ITEM_COUNT": "63" + "BOXTHRUSTREVERSE": "63", + "BOXMZTCCALIBRATE": "64", + "CHECKBOX_ITEM_COUNT": "65" }, "busIndex_e": { "_source": "inav/src/main/drivers/bus.h", @@ -1969,7 +1972,8 @@ "LED_OVERLAY_LANDING_FLASH": "3", "LED_OVERLAY_INDICATOR": "4", "LED_OVERLAY_WARNING": "5", - "LED_OVERLAY_STROBE": "6" + "LED_OVERLAY_STROBE": "6", + "LED_OVERLAY_RAINBOW": "7" }, "ledSpecialColorIds_e": { "_source": "inav/src/main/io/ledstrip.h", @@ -2485,7 +2489,8 @@ "PWM_TYPE_BRUSHED": "3", "PWM_TYPE_DSHOT150": "4", "PWM_TYPE_DSHOT300": "5", - "PWM_TYPE_DSHOT600": "6" + "PWM_TYPE_DSHOT600": "6", + "PWM_TYPE_SRXL2": "7" }, "motorStatus_e": { "_source": "inav/src/main/flight/mixer.h", @@ -2608,6 +2613,13 @@ "FW_LAUNCH_ABORTED": "10", "FW_LAUNCH_FLYING": "11" }, + "navFwWpTurnMode_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_FW_WP_TURN_DIRECT": "0", + "NAV_FW_WP_TURN_COORD_FLY_BY": "1", + "NAV_FW_WP_TURN_COORD_FLY_OVER": "2", + "NAV_FW_WP_TURN_COORD_FLY_INTO": "3" + }, "navigationEstimateStatus_e": { "_source": "inav/src/main/navigation/navigation_private.h", "EST_NONE": "0", @@ -3225,7 +3237,8 @@ "OSD_GPS_EXTRA_STATS": "169", "OSD_AUTO_SPEED": "170", "OSD_TERRAIN_AGL": "171", - "OSD_ITEM_COUNT": "172" + "OSD_MZTC_STATUS": "172", + "OSD_ITEM_COUNT": "173" }, "osd_sidebar_arrow_e": { "_source": "inav/src/main/io/osd_grid.c", @@ -3900,7 +3913,12 @@ "FUNCTION_CRSF_SENSOR": "(1 << 24)", "FUNCTION_MSP_OSD": "(1 << 25)", "FUNCTION_GIMBAL": "(1 << 26)", - "FUNCTION_GIMBAL_HEADTRACKER": "(1 << 27)" + "FUNCTION_GIMBAL_HEADTRACKER": "(1 << 27)", + "FUNCTION_MZTC_CAMERA": [ + "(1 << 28)", + "USE_MZTC" + ], + "FUNCTION_ESC_SRXL2": "(1 << 29)" }, "serialPortIdentifier_e": { "_source": "inav/src/main/io/serial.h", @@ -4097,6 +4115,23 @@ "DSMR_11ms_22ms": "226", "DSMR_5_5ms": "228" }, + "srxl2CalPhase_e": { + "_source": "inav/src/main/io/motor_srxl2.h", + "SRXL2_CAL_OFF": "0", + "SRXL2_CAL_WAIT_BATTERY": "1", + "SRXL2_CAL_SETTLE": "2", + "SRXL2_CAL_LOW": "3", + "SRXL2_CAL_HIGH_MANUAL": "4", + "SRXL2_CAL_LOW_MANUAL": "5" + }, + "srxl2CalResult_e": { + "_source": "inav/src/main/io/motor_srxl2.h", + "SRXL2_CAL_ACCEPTED": "0", + "SRXL2_CAL_REJECT_ARMED": "1", + "SRXL2_CAL_REJECT_NO_PORT": "2", + "SRXL2_CAL_REJECT_BATTERY_PRESENT": "3", + "SRXL2_CAL_REJECT_NO_VOLTAGE_SENSOR": "4" + }, "Srxl2ControlDataCommand": { "_source": "inav/src/main/rx/srxl2_types.h", "ChannelData": "0", @@ -4138,6 +4173,28 @@ "ListenForHandshake": "3", "Running": "4" }, + "srxl2State_e": { + "_source": "inav/src/main/io/motor_srxl2.c", + "SRXL2_DISABLED": "0", + "SRXL2_LISTENING": "1", + "SRXL2_POLLING": "2", + "SRXL2_FINALISING": "3", + "SRXL2_RUNNING": "4" + }, + "srxl2TelemetryField_e": { + "_source": "inav/src/main/io/motor_srxl2.h", + "SRXL2_TELEM_FIELD_RPM": "(1 << 0)", + "SRXL2_TELEM_FIELD_VOLTAGE": "(1 << 1)", + "SRXL2_TELEM_FIELD_CURRENT": "(1 << 2)", + "SRXL2_TELEM_FIELD_TEMP_FET": "(1 << 3)", + "SRXL2_TELEM_FIELD_TEMP_BEC": "(1 << 4)" + }, + "srxl2TelemetryRate_e": { + "_source": "inav/src/main/io/motor_srxl2.h", + "SRXL2_TELEM_1HZ": "0", + "SRXL2_TELEM_3HZ": "1", + "SRXL2_TELEM_2HZ": "2" + }, "stateFlags_t": { "_source": "inav/src/main/fc/runtime_config.h", "GPS_FIX_HOME": "(1 << 0)", @@ -4239,22 +4296,23 @@ }, "timId_e": { "_source": "inav/src/main/io/ledstrip.c", - "timBlink": "0", - "timLarson": "1", - "timBattery": "2", - "timRssi": "3", + "timRainbow": "0", + "timBlink": "1", + "timLarson": "2", + "timBattery": "3", + "timRssi": "4", "timGps": [ - "(4)", + "(5)", "USE_GPS" ], - "timWarning": "5", - "timIndicator": "6", + "timWarning": "6", + "timIndicator": "7", "timAnimation": [ - "(7)", + "(8)", "USE_LED_ANIMATION" ], - "timRing": "8", - "timTimerCount": "9" + "timRing": "9", + "timTimerCount": "10" }, "tristate_e": { "_source": "inav/src/main/common/tristate.h", @@ -4491,12 +4549,6 @@ "DISPLAY_WIDGET_AHI_STYLE_STAIRCASE": "0", "DISPLAY_WIDGET_AHI_STYLE_LINE": "1" }, - "wpFwTurnSmoothing_e": { - "_source": "inav/src/main/navigation/navigation.h", - "WP_TURN_SMOOTHING_OFF": "0", - "WP_TURN_SMOOTHING_ON": "1", - "WP_TURN_SMOOTHING_CUT": "2" - }, "wpMissionPlannerStatus_e": { "_source": "inav/src/main/navigation/navigation.h", "WP_PLAN_WAIT": "0", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 87168663713..86faf4f3d16 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -208,6 +208,7 @@ - [navDefaultAltitudeSensor_e](#enum-navdefaultaltitudesensor_e) - [navExtraArmingSafety_e](#enum-navextraarmingsafety_e) - [navFwLaunchStatus_e](#enum-navfwlaunchstatus_e) +- [navFwWpTurnMode_e](#enum-navfwwpturnmode_e) - [navigationEstimateStatus_e](#enum-navigationestimatestatus_e) - [navigationFSMEvent_t](#enum-navigationfsmevent_t) - [navigationFSMState_t](#enum-navigationfsmstate_t) @@ -334,11 +335,16 @@ - [SPIDevice](#enum-spidevice) - [Srxl2BindRequest](#enum-srxl2bindrequest) - [Srxl2BindType](#enum-srxl2bindtype) +- [srxl2CalPhase_e](#enum-srxl2calphase_e) +- [srxl2CalResult_e](#enum-srxl2calresult_e) - [Srxl2ControlDataCommand](#enum-srxl2controldatacommand) - [Srxl2DeviceId](#enum-srxl2deviceid) - [Srxl2DeviceType](#enum-srxl2devicetype) - [Srxl2PacketType](#enum-srxl2packettype) - [Srxl2State](#enum-srxl2state) +- [srxl2State_e](#enum-srxl2state_e) +- [srxl2TelemetryField_e](#enum-srxl2telemetryfield_e) +- [srxl2TelemetryRate_e](#enum-srxl2telemetryrate_e) - [stateFlags_t](#enum-stateflags_t) - [stickPositions_e](#enum-stickpositions_e) - [systemState_e](#enum-systemstate_e) @@ -377,7 +383,6 @@ - [warningLedState_e](#enum-warningledstate_e) - [widgetAHIOptions_t](#enum-widgetahioptions_t) - [widgetAHIStyle_e](#enum-widgetahistyle_e) -- [wpFwTurnSmoothing_e](#enum-wpfwturnsmoothing_e) - [wpMissionPlannerStatus_e](#enum-wpmissionplannerstatus_e) - [zeroCalibrationState_e](#enum-zerocalibrationstate_e) @@ -529,7 +534,8 @@ | `ADJUSTMENT_FW_LEVEL_TRIM` | 58 | | | `ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX` | 59 | | | `ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE` | 60 | | -| `ADJUSTMENT_FUNCTION_COUNT` | 61 | | +| `ADJUSTMENT_MZTC_ZOOM` | 61 | | +| `ADJUSTMENT_FUNCTION_COUNT` | 62 | | --- ## `adjustmentMode_e` @@ -1203,7 +1209,9 @@ | `BOXAUTOSPEED` | 60 | | | `BOXTERRAINAGLHOLD` | 61 | | | `BOXINFLIGHTMENU` | 62 | | -| `CHECKBOX_ITEM_COUNT` | 63 | | +| `BOXTHRUSTREVERSE` | 63 | | +| `BOXMZTCCALIBRATE` | 64 | | +| `CHECKBOX_ITEM_COUNT` | 65 | | --- ## `busIndex_e` @@ -3052,6 +3060,7 @@ | `LED_OVERLAY_INDICATOR` | 4 | | | `LED_OVERLAY_WARNING` | 5 | | | `LED_OVERLAY_STROBE` | 6 | | +| `LED_OVERLAY_RAINBOW` | 7 | | --- ## `ledSpecialColorIds_e` @@ -3663,6 +3672,7 @@ | `PWM_TYPE_DSHOT150` | 4 | | | `PWM_TYPE_DSHOT300` | 5 | | | `PWM_TYPE_DSHOT600` | 6 | | +| `PWM_TYPE_SRXL2` | 7 | | --- ## `motorStatus_e` @@ -3880,6 +3890,18 @@ | `FW_LAUNCH_ABORTED` | 10 | | | `FW_LAUNCH_FLYING` | 11 | | +--- +## `navFwWpTurnMode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_FW_WP_TURN_DIRECT` | 0 | | +| `NAV_FW_WP_TURN_COORD_FLY_BY` | 1 | | +| `NAV_FW_WP_TURN_COORD_FLY_OVER` | 2 | | +| `NAV_FW_WP_TURN_COORD_FLY_INTO` | 3 | | + --- ## `navigationEstimateStatus_e` @@ -4688,7 +4710,8 @@ | `OSD_GPS_EXTRA_STATS` | 169 | | | `OSD_AUTO_SPEED` | 170 | | | `OSD_TERRAIN_AGL` | 171 | | -| `OSD_ITEM_COUNT` | 172 | | +| `OSD_MZTC_STATUS` | 172 | | +| `OSD_ITEM_COUNT` | 173 | | --- ## `osd_sidebar_arrow_e` @@ -5637,6 +5660,8 @@ | `FUNCTION_MSP_OSD` | (1 << 25) | | | `FUNCTION_GIMBAL` | (1 << 26) | | | `FUNCTION_GIMBAL_HEADTRACKER` | (1 << 27) | | +| `FUNCTION_MZTC_CAMERA` | (1 << 28) | USE_MZTC | +| `FUNCTION_ESC_SRXL2` | (1 << 29) | | --- ## `serialPortIdentifier_e` @@ -5948,6 +5973,33 @@ | `DSMR_11ms_22ms` | 226 | | | `DSMR_5_5ms` | 228 | | +--- +## `srxl2CalPhase_e` + +> Source: ../../../src/main/io/motor_srxl2.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SRXL2_CAL_OFF` | 0 | | +| `SRXL2_CAL_WAIT_BATTERY` | 1 | | +| `SRXL2_CAL_SETTLE` | 2 | | +| `SRXL2_CAL_LOW` | 3 | | +| `SRXL2_CAL_HIGH_MANUAL` | 4 | | +| `SRXL2_CAL_LOW_MANUAL` | 5 | | + +--- +## `srxl2CalResult_e` + +> Source: ../../../src/main/io/motor_srxl2.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SRXL2_CAL_ACCEPTED` | 0 | | +| `SRXL2_CAL_REJECT_ARMED` | 1 | | +| `SRXL2_CAL_REJECT_NO_PORT` | 2 | | +| `SRXL2_CAL_REJECT_BATTERY_PRESENT` | 3 | | +| `SRXL2_CAL_REJECT_NO_VOLTAGE_SENSOR` | 4 | | + --- ## `Srxl2ControlDataCommand` @@ -6014,6 +6066,43 @@ | `ListenForHandshake` | 3 | | | `Running` | 4 | | +--- +## `srxl2State_e` + +> Source: ../../../src/main/io/motor_srxl2.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SRXL2_DISABLED` | 0 | | +| `SRXL2_LISTENING` | 1 | | +| `SRXL2_POLLING` | 2 | | +| `SRXL2_FINALISING` | 3 | | +| `SRXL2_RUNNING` | 4 | | + +--- +## `srxl2TelemetryField_e` + +> Source: ../../../src/main/io/motor_srxl2.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SRXL2_TELEM_FIELD_RPM` | (1 << 0) | | +| `SRXL2_TELEM_FIELD_VOLTAGE` | (1 << 1) | | +| `SRXL2_TELEM_FIELD_CURRENT` | (1 << 2) | | +| `SRXL2_TELEM_FIELD_TEMP_FET` | (1 << 3) | | +| `SRXL2_TELEM_FIELD_TEMP_BEC` | (1 << 4) | | + +--- +## `srxl2TelemetryRate_e` + +> Source: ../../../src/main/io/motor_srxl2.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SRXL2_TELEM_1HZ` | 0 | | +| `SRXL2_TELEM_3HZ` | 1 | | +| `SRXL2_TELEM_2HZ` | 2 | | + --- ## `stateFlags_t` @@ -6176,16 +6265,17 @@ | Enumerator | Value | Condition | |---|---:|---| -| `timBlink` | 0 | | -| `timLarson` | 1 | | -| `timBattery` | 2 | | -| `timRssi` | 3 | | -| `timGps` | (4) | USE_GPS | -| `timWarning` | 5 | | -| `timIndicator` | 6 | | -| `timAnimation` | (7) | USE_LED_ANIMATION | -| `timRing` | 8 | | -| `timTimerCount` | 9 | | +| `timRainbow` | 0 | | +| `timBlink` | 1 | | +| `timLarson` | 2 | | +| `timBattery` | 3 | | +| `timRssi` | 4 | | +| `timGps` | (5) | USE_GPS | +| `timWarning` | 6 | | +| `timIndicator` | 7 | | +| `timAnimation` | (8) | USE_LED_ANIMATION | +| `timRing` | 9 | | +| `timTimerCount` | 10 | | --- ## `tristate_e` @@ -6557,17 +6647,6 @@ | `DISPLAY_WIDGET_AHI_STYLE_STAIRCASE` | 0 | | | `DISPLAY_WIDGET_AHI_STYLE_LINE` | 1 | | ---- -## `wpFwTurnSmoothing_e` - -> Source: ../../../src/main/navigation/navigation.h - -| Enumerator | Value | Condition | -|---|---:|---| -| `WP_TURN_SMOOTHING_OFF` | 0 | | -| `WP_TURN_SMOOTHING_ON` | 1 | | -| `WP_TURN_SMOOTHING_CUT` | 2 | | - --- ## `wpMissionPlannerStatus_e` diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 176833915ad..3ebd980b4c2 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -1,7 +1,7 @@ { "version": { "major": 2, - "minor": 1, + "minor": 7, "patch": 0 }, "messages": { @@ -11674,6 +11674,344 @@ }, "notes": "Requires a receiver using MSP as the protocol, sends MSP2_RX_BIND to the receiver.", "description": "Initiates binding for MSP receivers (mLRS)." + }, + "MSP2_MZTC_CONFIG": { + "code": 8768, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "preset", + "ctype": "uint8_t", + "units": "", + "desc": "Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME." + }, + { + "name": "palette_mode", + "ctype": "uint8_t", + "units": "", + "desc": "Colour palette. See `mztcPaletteMode_e`. 0-13." + }, + { + "name": "auto_shutter", + "ctype": "uint8_t", + "units": "", + "desc": "Automatic shutter policy. See `mztcShutterMode_e`. 0-2." + }, + { + "name": "digital_enhancement", + "ctype": "uint8_t", + "units": "%", + "desc": "Digital enhancement level. 0-100." + }, + { + "name": "spatial_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Spatial denoising level. 0-100." + }, + { + "name": "temporal_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Temporal denoising level. 0-100." + }, + { + "name": "brightness", + "ctype": "uint8_t", + "units": "%", + "desc": "Image brightness. 0-100." + }, + { + "name": "contrast", + "ctype": "uint8_t", + "units": "%", + "desc": "Image contrast. 0-100." + }, + { + "name": "zoom_level", + "ctype": "uint8_t", + "units": "", + "desc": "Digital zoom. See `mztcZoomLevel_e`. 0-3." + }, + { + "name": "mirror_mode", + "ctype": "uint8_t", + "units": "", + "desc": "Image mirroring. See `mztcMirrorMode_e`. 0-3." + }, + { + "name": "ffc_interval", + "ctype": "uint8_t", + "units": "minutes", + "desc": "Automatic shutter interval in minutes. 1-60. The camera runs the schedule itself." + } + ] + }, + "notes": "Requires `USE_MZTC`. Fixed 12 byte reply. The serial port and its baud rate are not in this payload. They come from the Ports tab. Each field is written with the `sbufWrite*` helpers. The layout never depends on compiler padding.", + "description": "Reads the MassZero thermal camera configuration." + }, + "MSP2_MZTC_STATUS": { + "code": 8769, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "status", + "ctype": "uint8_t", + "units": "", + "desc": "Camera state. 0 offline, 1 initializing, 2 ready, 3 capturing, 4 calibrating, 5 error, 6 alert, 7 recording." + }, + { + "name": "preset", + "ctype": "uint8_t", + "units": "", + "desc": "Purpose preset currently in effect. See `mztcPreset_e`." + }, + { + "name": "connected", + "ctype": "uint8_t", + "units": "", + "desc": "Set once the camera has answered a command. Opening the serial port alone does not set it." + }, + { + "name": "connection_quality", + "ctype": "uint8_t", + "units": "%", + "desc": "Share of recent identity probes the camera answered. 0-100." + }, + { + "name": "last_calibration", + "ctype": "uint16_t", + "units": "minutes", + "desc": "Time since the last flat field correction. Saturates at 65535." + }, + { + "name": "error_flags", + "ctype": "uint8_t", + "units": "", + "desc": "Bit field. 0x01 communication, 0x02 calibration, 0x04 temperature, 0x08 memory, 0x10 timeout, 0x20 invalid config." + } + ] + }, + "notes": "Requires `USE_MZTC`. Fixed 7 byte reply.", + "description": "Reads the live state of the MassZero thermal camera link." + }, + "MSP2_SET_MZTC_CONFIG": { + "code": 8770, + "mspv": 2, + "request": { + "payload": [ + { + "name": "preset", + "ctype": "uint8_t", + "units": "", + "desc": "Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME." + }, + { + "name": "palette_mode", + "ctype": "uint8_t", + "units": "", + "desc": "Colour palette. See `mztcPaletteMode_e`. 0-13." + }, + { + "name": "auto_shutter", + "ctype": "uint8_t", + "units": "", + "desc": "Automatic shutter policy. See `mztcShutterMode_e`. 0-2." + }, + { + "name": "digital_enhancement", + "ctype": "uint8_t", + "units": "%", + "desc": "Digital enhancement level. 0-100." + }, + { + "name": "spatial_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Spatial denoising level. 0-100." + }, + { + "name": "temporal_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Temporal denoising level. 0-100." + }, + { + "name": "brightness", + "ctype": "uint8_t", + "units": "%", + "desc": "Image brightness. 0-100." + }, + { + "name": "contrast", + "ctype": "uint8_t", + "units": "%", + "desc": "Image contrast. 0-100." + }, + { + "name": "zoom_level", + "ctype": "uint8_t", + "units": "", + "desc": "Digital zoom. See `mztcZoomLevel_e`. 0-3." + }, + { + "name": "mirror_mode", + "ctype": "uint8_t", + "units": "", + "desc": "Image mirroring. See `mztcMirrorMode_e`. 0-3." + }, + { + "name": "ffc_interval", + "ctype": "uint8_t", + "units": "minutes", + "desc": "Automatic shutter interval in minutes. 1-60. The camera runs the schedule itself." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 11 bytes. The serial port and its baud rate are not in this payload. They come from the Ports tab. The whole request is validated against the `MZTC_*` limits before any field is applied. A rejected request leaves the running configuration untouched.", + "description": "Writes the MassZero thermal camera configuration." + }, + "MSP2_SET_MZTC_PRESET": { + "code": 8771, + "mspv": 2, + "request": { + "payload": [ + { + "name": "preset", + "ctype": "uint8_t", + "units": "", + "desc": "Purpose preset. See `mztcPreset_e`. 0 CUSTOM, 1 GENERAL, 2 FIRE, 3 SEARCH, 4 SURVEILLANCE, 5 INSPECTION, 6 MARITIME." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 1 byte.", + "description": "Applies a purpose preset to the MassZero thermal camera. A preset writes the palette, brightness, contrast, digital enhancement, both denoise levels, the shutter mode and the correction interval. CUSTOM writes nothing." + }, + "MSP2_SET_MZTC_PALETTE": { + "code": 8772, + "mspv": 2, + "request": { + "payload": [ + { + "name": "palette", + "ctype": "uint8_t", + "units": "", + "desc": "Colour palette. See `mztcPaletteMode_e`. 0-13." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 1 byte. Returns an error if the camera is not connected.", + "description": "Sets the MassZero thermal camera colour palette." + }, + "MSP2_SET_MZTC_ZOOM": { + "code": 8773, + "mspv": 2, + "request": { + "payload": [ + { + "name": "zoom_level", + "ctype": "uint8_t", + "units": "", + "desc": "Digital zoom. See `mztcZoomLevel_e`. 0-3." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 1 byte. Returns an error if the camera is not connected.", + "description": "Sets the MassZero thermal camera digital zoom level." + }, + "MSP2_SET_MZTC_SHUTTER": { + "code": 8774, + "mspv": 2, + "request": { + "payload": [ + { + "name": "trigger", + "ctype": "uint8_t", + "units": "", + "desc": "Ignored. The command itself is the trigger." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Accepts 0 or 1 bytes. A manual shutter cycle is the flat field correction on this camera. The `mztc_calibrate` CLI command performs the same operation.", + "description": "Triggers a manual shutter cycle for a flat field correction." + }, + "MSP2_SET_MZTC_IMAGE_PARAMS": { + "code": 8775, + "mspv": 2, + "request": { + "payload": [ + { + "name": "brightness", + "ctype": "uint8_t", + "units": "%", + "desc": "Image brightness. 0-100." + }, + { + "name": "contrast", + "ctype": "uint8_t", + "units": "%", + "desc": "Image contrast. 0-100." + }, + { + "name": "enhancement", + "ctype": "uint8_t", + "units": "%", + "desc": "Digital enhancement. 0-100." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 3 bytes. Returns an error if the camera is not connected.", + "description": "Sets the MassZero thermal camera image parameters." + }, + "MSP2_SET_MZTC_CORRECTION": { + "code": 8776, + "mspv": 2, + "request": { + "payload": [ + { + "name": "spatial_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Spatial denoising. 0-100." + }, + { + "name": "temporal_denoise", + "ctype": "uint8_t", + "units": "%", + "desc": "Temporal denoising. 0-100." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Expects 2 bytes. Returns an error if the camera is not connected.", + "description": "Sets the MassZero thermal camera denoising parameters." + }, + "MSP2_SET_MZTC_VIGNETTING": { + "code": 8777, + "mspv": 2, + "request": { + "payload": [ + { + "name": "trigger", + "ctype": "uint8_t", + "units": "", + "desc": "Ignored. The command itself is the trigger." + } + ] + }, + "reply": null, + "notes": "Requires `USE_MZTC`. Accepts 0 or 1 bytes. The camera manual requires the lens to be pointed at a uniform surface before this runs, so it is an action and never a stored setting.", + "description": "Runs one vignetting correction on the camera." } } } diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 054af703408..b6c103f2079 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -1,4 +1,3 @@ - main_sources(COMMON_SRC main.c @@ -64,6 +63,7 @@ main_sources(COMMON_SRC config/parameter_group.h config/general_settings.c config/general_settings.h + config/mztc_camera.h drivers/1-wire.c drivers/1-wire.h @@ -422,6 +422,11 @@ main_sources(COMMON_SRC io/osd/custom_elements.c + io/mztc_camera.c + io/mztc_camera.h + io/osd/mztc_camera_osd.c + io/osd/mztc_camera_osd.h + mavlink/mavlink_command.c mavlink/mavlink_command.h mavlink/mavlink_guided.c @@ -444,6 +449,7 @@ main_sources(COMMON_SRC msp/msp_serial.c msp/msp_serial.h + msp/msp_mztc.h programming/logic_condition.c programming/logic_condition.h @@ -490,7 +496,6 @@ main_sources(COMMON_SRC rx/sim.c rx/sim.h - scheduler/scheduler.c scheduler/scheduler.h diff --git a/src/main/config/mztc_camera.h b/src/main/config/mztc_camera.h new file mode 100644 index 00000000000..3e892cf699c --- /dev/null +++ b/src/main/config/mztc_camera.h @@ -0,0 +1,189 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR ANY PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include + +#ifdef USE_MZTC + +#include "config/parameter_group.h" +#include "drivers/time.h" + +// Purpose presets. A preset is a named bundle of the image settings below. It +// is applied on the flight controller and writes ordinary camera commands. The +// camera has no preset mechanism of its own. +// +// MZTC_PRESET_CUSTOM writes nothing, so a hand-tuned configuration survives. +// Changing any setting a preset owns switches the selection back to it. +typedef enum { + MZTC_PRESET_CUSTOM = 0, // Leave every setting as the user left it + MZTC_PRESET_GENERAL, // Balanced, for ordinary flying + MZTC_PRESET_FIRE, // Flat mid-tones so hot spikes dominate + MZTC_PRESET_SEARCH, // Lift a small warm target out of ambient + MZTC_PRESET_SURVEILLANCE, // Loiter work, where frame averaging helps + MZTC_PRESET_INSPECTION, // Read gradients across a surface + MZTC_PRESET_MARITIME // Warm target on a large uniform cold field +} mztcPreset_e; + +// MassZero Thermal Camera color palettes +typedef enum { + MZTC_PALETTE_WHITE_HOT = 0, // Default white hot + MZTC_PALETTE_BLACK_HOT = 1, // Black hot + MZTC_PALETTE_FUSION_1 = 2, // Fusion 1 + MZTC_PALETTE_RAINBOW = 3, // Rainbow + MZTC_PALETTE_FUSION_2 = 4, // Fusion 2 + MZTC_PALETTE_IRON_RED_1 = 5, // Iron red 1 + MZTC_PALETTE_IRON_RED_2 = 6, // Iron red 2 + MZTC_PALETTE_SEPIA = 7, // Sepia + MZTC_PALETTE_COLOR_1 = 8, // Color 1 + MZTC_PALETTE_COLOR_2 = 9, // Color 2 + MZTC_PALETTE_ICE_FIRE = 10, // Ice fire + MZTC_PALETTE_RAIN = 11, // Rain + MZTC_PALETTE_GREEN_HOT = 12, // Green hot + MZTC_PALETTE_RED_HOT = 13 // Red hot +} mztcPaletteMode_e; + +// MassZero Thermal Camera zoom levels +typedef enum { + MZTC_ZOOM_1X = 0, // 1x zoom (default) + MZTC_ZOOM_2X = 1, // 2x zoom + MZTC_ZOOM_4X = 2, // 4x zoom (per thermal camera docs) + MZTC_ZOOM_8X = 3 // 8x zoom (per thermal camera docs) +} mztcZoomLevel_e; + +// MassZero Thermal Camera mirror modes +typedef enum { + MZTC_MIRROR_NONE = 0, // No mirroring (default) + MZTC_MIRROR_HORIZONTAL = 1, // Horizontal mirror + MZTC_MIRROR_VERTICAL = 2, // Vertical mirror + MZTC_MIRROR_CENTRAL = 3 // Both horizontal and vertical +} mztcMirrorMode_e; + +// MassZero Thermal Camera auto shutter modes. +// +// The camera expects 0x01 to 0x03 on the wire and rejects 0x00 as out of +// range, so the driver adds MZTC_SHUTTER_WIRE_OFFSET when it sends the value. +// These enum values stay zero-based because settings.yaml indexes its lookup +// table from zero. +typedef enum { + MZTC_SHUTTER_TEMP_ONLY = 0, // Camera wire value 0x01 + MZTC_SHUTTER_TIME_ONLY = 1, // Camera wire value 0x02 + MZTC_SHUTTER_TIME_AND_TEMP = 2 // Camera wire value 0x03, the default +} mztcShutterMode_e; + +#define MZTC_SHUTTER_WIRE_OFFSET 1 + +// MassZero Thermal Camera limits. +// +// These are the single source of truth for the valid ranges. settings.yaml +// references them for the CLI bounds. The MSP handlers validate against them. +// A value the CLI rejects cannot be smuggled in over MSP. +#define MZTC_MIN_FFC_INTERVAL 1 // Minimum 1 minute +#define MZTC_MAX_FFC_INTERVAL 60 // Maximum 60 minutes +#define MZTC_MIN_PERCENT 0 +#define MZTC_MAX_PERCENT 100 + +// MassZero Thermal Camera configuration structure +// The serial port and its baud rate come from the Ports tab through +// findSerialPortConfig(FUNCTION_MZTC_CAMERA), the same way every other serial +// peripheral in INAV works. Assigning the function is what enables the camera, +// so there is no separate enable, port or baudrate setting to keep in step. +typedef struct mztcConfig_s { + uint8_t preset; // Purpose preset, see mztcPreset_e + uint8_t palette_mode; // Color palette + uint8_t auto_shutter; // Auto shutter mode + uint8_t digital_enhancement; // Digital enhancement (0-100) + uint8_t spatial_denoise; // Spatial denoising (0-100) + uint8_t temporal_denoise; // Temporal denoising (0-100) + uint8_t brightness; // Brightness (0-100) + uint8_t contrast; // Contrast (0-100) + uint8_t zoom_level; // Digital zoom level + uint8_t mirror_mode; // Image mirroring + uint8_t ffc_interval; // Automatic shutter interval, in minutes +} mztcConfig_t; + +// MassZero Thermal Camera status structure +typedef struct mztcStatus_s { + uint8_t status; // Camera status + uint8_t preset; // Purpose preset in effect + bool connected; // Connection status + uint8_t connection_quality; // Connection quality indicator + uint16_t last_calibration; // Minutes since last calibration (saturates at UINT16_MAX) + uint8_t error_flags; // Error status flags +} mztcStatus_t; + +// MassZero Thermal Camera status values +#define MZTC_STATUS_OFFLINE 0x00 +#define MZTC_STATUS_INITIALIZING 0x01 +#define MZTC_STATUS_READY 0x02 +#define MZTC_STATUS_CAPTURING 0x03 +#define MZTC_STATUS_CALIBRATING 0x04 +#define MZTC_STATUS_ERROR 0x05 +#define MZTC_STATUS_ALERT 0x06 +#define MZTC_STATUS_RECORDING 0x07 + +// MassZero Thermal Camera error flags +#define MZTC_ERROR_COMMUNICATION 0x01 +#define MZTC_ERROR_CALIBRATION 0x02 +#define MZTC_ERROR_TEMPERATURE 0x04 +#define MZTC_ERROR_MEMORY 0x08 +#define MZTC_ERROR_TIMEOUT 0x10 +#define MZTC_ERROR_INVALID_CONFIG 0x20 + +// Wire framing constants for the camera serial protocol. +// Layout: begin(1) size(1) addr(1) class(1) subclass(1) flags(1) data(N) +// checksum(1) end(1). The size field is N+4 and covers addr through +// checksum. The total byte count on the wire is size+4. +#define MZTC_PACKET_BEGIN 0xF0 +#define MZTC_PACKET_END 0xFF +#define MZTC_DEVICE_ADDR 0x36 +#define MZTC_MAX_DATA_LEN 14 +#define MZTC_PACKET_OVERHEAD 8 // Everything that is not payload +#define MZTC_MIN_PACKET_LEN MZTC_PACKET_OVERHEAD +#define MZTC_MAX_PACKET_LEN (MZTC_PACKET_OVERHEAD + MZTC_MAX_DATA_LEN) +#define MZTC_SIZE_FIELD_OFFSET 4 // size = data_len + 4 + +// Parameter group declaration +PG_DECLARE(mztcConfig_t, mztcConfig); + +// Function declarations +void mztcInit(void); +void mztcUpdate(timeUs_t currentTimeUs); +bool mztcIsEnabled(void); +mztcStatus_t* mztcGetStatus(void); +bool mztcTriggerCalibration(void); +bool mztcSetPreset(mztcPreset_e preset); +bool mztcSetPalette(mztcPaletteMode_e palette); +bool mztcSetZoom(mztcZoomLevel_e zoom); +bool mztcSetImageParams(uint8_t brightness, uint8_t contrast, uint8_t enhancement); +bool mztcSetDenoising(uint8_t spatial, uint8_t temporal); +bool mztcIsConnected(void); +void mztcRequestReconnect(void); +bool mztcSaveConfiguration(void); +bool mztcRestoreDefaults(void); +bool mztcTriggerVignettingCorrection(void); +bool mztcConfigIsValid(const mztcConfig_t *cfg); + +// Serial framing helpers. Exposed so the unit test can exercise the wire +// format without a serial port; see mztc_camera_unittest.cc. +uint8_t mztcBuildPacket(uint8_t *out, uint8_t class_cmd, uint8_t subclass_cmd, + uint8_t flags, const uint8_t *data, uint8_t data_len); +bool mztcPacketIsValid(const uint8_t *packet, uint8_t len); + +#endif // USE_MZTC diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index fe52a22e63f..58676574d32 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -136,7 +136,12 @@ #define PG_DRONECAN_CONFIG 1045 #define PG_TERRAIN_NAV_CONFIG 1046 #define PG_DRONECAN_DNA_SERVER 1047 // Separate PG so we don't wipe user settings if the allocation table changes +#ifdef USE_MZTC +#define PG_MZTC_CAMERA_CONFIG 1048 +#define PG_INAV_END PG_MZTC_CAMERA_CONFIG +#else #define PG_INAV_END PG_DRONECAN_DNA_SERVER +#endif // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ab65ab4262d..9da5e6624e2 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -97,6 +97,7 @@ bool cliMode = false; #include "io/osd/custom_elements.h" #include "io/motor_srxl2.h" #include "io/serial.h" +#include "io/mztc_camera.h" #include "fc/fc_msp_box.h" @@ -157,6 +158,21 @@ static uint16_t cliDelayMs = 0; static void cliAssert(char *cmdline); #endif +#ifdef USE_MZTC +static void cliMztc(char *cmdline); +static void cliMztcPreset(char *cmdline); +static void cliMztcConfig(char *cmdline); +static void cliMztcPalette(char *cmdline); +static void cliMztcZoom(char *cmdline); +static void cliMztcEnhancement(char *cmdline); +static void cliMztcDenoise(char *cmdline); +static void cliMztcCalibrate(char *cmdline); +static void cliMztcReconnect(char *cmdline); +static void cliMztcSave(char *cmdline); +static void cliMztcDefaults(char *cmdline); +static void cliMztcVignetting(char *cmdline); +#endif + #ifdef USE_CLI_BATCH static bool commandBatchActive = false; static bool commandBatchError = false; @@ -5030,6 +5046,270 @@ static void printBootLog(char *cmdline __attribute__((unused))) { } #endif +#ifdef USE_MZTC +// MassZero Thermal Camera CLI commands. +// +// Bounds come from the MZTC_* limits in config/mztc_camera.h. settings.yaml +// and the MSP handlers use the same values. Every entry point accepts exactly +// the same range. +static const char * const mztcPresetNames[] = { + "CUSTOM", "GENERAL", "FIRE", "SEARCH", "SURVEILLANCE", "INSPECTION", "MARITIME" +}; + +static void cliMztc(char *cmdline) +{ + UNUSED(cmdline); + + if (!mztcIsEnabled()) { + cliPrintLine("MassZero Thermal Camera is disabled"); + return; + } + + const mztcStatus_t *status = mztcGetStatus(); + + cliPrintLine("MassZero Thermal Camera status:"); + cliPrintLinef(" Connected: %s", status->connected ? "YES" : "NO"); + cliPrintLinef(" State: %u", status->status); + cliPrintLinef(" Preset: %u (%s)", status->preset, + status->preset < ARRAYLEN(mztcPresetNames) ? mztcPresetNames[status->preset] : "?"); + cliPrintLinef(" Link quality: %u%%", status->connection_quality); + + uint8_t idLen = 0; + const uint8_t *id = mztcGetDeviceId(&idLen); + if (id) { + cliPrintf(" Device ID:"); + for (uint8_t i = 0; i < idLen; i++) { + cliPrintf(" %02X", id[i]); + } + cliPrintLinefeed(); + } else { + cliPrintLine(" Device ID: not reported"); + } + + cliPrintLinef(" Minutes since calibration: %u", status->last_calibration); + cliPrintLinef(" Error flags: 0x%02X", status->error_flags); +} + +static void cliMztcPreset(char *cmdline) +{ + if (isEmpty(cmdline)) { + const uint8_t current = mztcGetStatus()->preset; + cliPrintLinef("Current preset: %u (%s)", current, + current < ARRAYLEN(mztcPresetNames) ? mztcPresetNames[current] : "?"); + for (unsigned i = 0; i < ARRAYLEN(mztcPresetNames); i++) { + cliPrintLinef(" %u %s", i, mztcPresetNames[i]); + } + return; + } + + const int preset = fastA2I(cmdline); + if (preset < 0 || preset > MZTC_PRESET_MARITIME) { + cliPrintLinef("Invalid preset. Use 0-%d", MZTC_PRESET_MARITIME); + return; + } + + if (mztcSetPreset((mztcPreset_e)preset)) { + cliPrintLinef("Preset set to %d (%s)", preset, mztcPresetNames[preset]); + if (preset != MZTC_PRESET_CUSTOM) { + cliPrintLine("Palette, brightness, contrast, enhancement, denoise, shutter and interval updated"); + } + } else { + cliPrintLine("Failed to set preset"); + } +} + +static void cliMztcConfig(char *cmdline) +{ + if (isEmpty(cmdline)) { + const mztcConfig_t *config = mztcConfig(); + cliPrintLinef("brightness=%u contrast=%u enhancement=%u", + config->brightness, config->contrast, config->digital_enhancement); + return; + } + + char *brightnessStr = strtok(cmdline, " "); + char *contrastStr = strtok(NULL, " "); + char *enhancementStr = strtok(NULL, " "); + + if (!brightnessStr || !contrastStr || !enhancementStr) { + cliPrintLine("Usage: mztc_config [brightness] [contrast] [enhancement]"); + return; + } + + const int brightness = fastA2I(brightnessStr); + const int contrast = fastA2I(contrastStr); + const int enhancement = fastA2I(enhancementStr); + + if (brightness < MZTC_MIN_PERCENT || brightness > MZTC_MAX_PERCENT || + contrast < MZTC_MIN_PERCENT || contrast > MZTC_MAX_PERCENT || + enhancement < MZTC_MIN_PERCENT || enhancement > MZTC_MAX_PERCENT) { + cliPrintLinef("Values must be %d-%d", MZTC_MIN_PERCENT, MZTC_MAX_PERCENT); + return; + } + + if (mztcSetImageParams(brightness, contrast, enhancement)) { + cliPrintLinef("brightness=%d contrast=%d enhancement=%d", brightness, contrast, enhancement); + } else { + cliPrintLine("Failed to set image parameters"); + } +} + +static void cliMztcPalette(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliPrintLinef("Current palette: %u", mztcConfig()->palette_mode); + return; + } + + const int palette = fastA2I(cmdline); + if (palette < 0 || palette > MZTC_PALETTE_RED_HOT) { + cliPrintLinef("Invalid palette. Use 0-%d", MZTC_PALETTE_RED_HOT); + return; + } + + if (mztcSetPalette((mztcPaletteMode_e)palette)) { + cliPrintLinef("Palette set to %d", palette); + } else { + cliPrintLine("Failed to set palette"); + } +} + +static void cliMztcZoom(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliPrintLinef("Current zoom: %u", mztcConfig()->zoom_level); + return; + } + + const int zoom = fastA2I(cmdline); + if (zoom < 0 || zoom > MZTC_ZOOM_8X) { + cliPrintLinef("Invalid zoom level. Use 0-%d", MZTC_ZOOM_8X); + return; + } + + if (mztcSetZoom((mztcZoomLevel_e)zoom)) { + cliPrintLinef("Zoom set to %d", zoom); + } else { + cliPrintLine("Failed to set zoom"); + } +} + +static void cliMztcEnhancement(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliPrintLinef("Current enhancement: %u", mztcConfig()->digital_enhancement); + return; + } + + const int value = fastA2I(cmdline); + if (value < MZTC_MIN_PERCENT || value > MZTC_MAX_PERCENT) { + cliPrintLinef("Value must be %d-%d", MZTC_MIN_PERCENT, MZTC_MAX_PERCENT); + return; + } + + const mztcConfig_t *config = mztcConfig(); + if (mztcSetImageParams(config->brightness, config->contrast, value)) { + cliPrintLinef("Enhancement set to %d", value); + } else { + cliPrintLine("Failed to set enhancement"); + } +} + +static void cliMztcDenoise(char *cmdline) +{ + if (isEmpty(cmdline)) { + const mztcConfig_t *config = mztcConfig(); + cliPrintLinef("spatial=%u temporal=%u", config->spatial_denoise, config->temporal_denoise); + return; + } + + char *spatialStr = strtok(cmdline, " "); + char *temporalStr = strtok(NULL, " "); + + if (!spatialStr || !temporalStr) { + cliPrintLine("Usage: mztc_denoise [spatial] [temporal]"); + return; + } + + const int spatial = fastA2I(spatialStr); + const int temporal = fastA2I(temporalStr); + + if (spatial < MZTC_MIN_PERCENT || spatial > MZTC_MAX_PERCENT || + temporal < MZTC_MIN_PERCENT || temporal > MZTC_MAX_PERCENT) { + cliPrintLinef("Values must be %d-%d", MZTC_MIN_PERCENT, MZTC_MAX_PERCENT); + return; + } + + if (mztcSetDenoising(spatial, temporal)) { + cliPrintLinef("spatial=%d temporal=%d", spatial, temporal); + } else { + cliPrintLine("Failed to set denoising"); + } +} + +// A manual shutter cycle is the flat field correction on this camera. One +// command covers both. +static void cliMztcCalibrate(char *cmdline) +{ + UNUSED(cmdline); + + if (mztcTriggerCalibration()) { + cliPrintLine("Calibration (manual shutter) triggered"); + } else { + cliPrintLine("Failed to trigger calibration"); + } +} + +static void cliMztcReconnect(char *cmdline) +{ + UNUSED(cmdline); + + if (mztcIsEnabled()) { + mztcRequestReconnect(); + cliPrintLine("MZTC: forcing reconnection"); + } else { + cliPrintLine("MZTC: camera is disabled"); + } +} + +// Both of these act on the camera's own flash. The INAV save command has no +// effect on them. +static void cliMztcSave(char *cmdline) +{ + UNUSED(cmdline); + + if (mztcSaveConfiguration()) { + cliPrintLine("Configuration saved to camera flash"); + } else { + cliPrintLine("Failed to save configuration to the camera"); + } +} + +static void cliMztcDefaults(char *cmdline) +{ + UNUSED(cmdline); + + if (mztcRestoreDefaults()) { + cliPrintLine("Camera restored to factory defaults"); + } else { + cliPrintLine("Failed to restore camera defaults"); + } +} + +// Point the lens at a uniform surface before running this. The camera +// superimposes whatever it is looking at onto the correction otherwise. +static void cliMztcVignetting(char *cmdline) +{ + UNUSED(cmdline); + + if (mztcTriggerVignettingCorrection()) { + cliPrintLine("Vignetting correction triggered. Point the lens at a uniform surface first."); + } else { + cliPrintLine("Failed to trigger vignetting correction"); + } +} +#endif // USE_MZTC + static void cliHelp(char *cmdline); // should be sorted a..z for bsearch() @@ -5107,6 +5387,20 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("map", "configure rc channel order", "[]", cliMap), CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), +#ifdef USE_MZTC + CLI_COMMAND_DEF("mztc", "MassZero Thermal Camera status", NULL, cliMztc), + CLI_COMMAND_DEF("mztc_calibrate", "trigger a manual shutter / flat field correction", NULL, cliMztcCalibrate), + CLI_COMMAND_DEF("mztc_config", "configure camera parameters, each 0-100", "[brightness] [contrast] [enhancement]", cliMztcConfig), + CLI_COMMAND_DEF("mztc_defaults", "restore the camera to its factory defaults", NULL, cliMztcDefaults), + CLI_COMMAND_DEF("mztc_denoise", "set denoising parameters", "[spatial] [temporal]", cliMztcDenoise), + CLI_COMMAND_DEF("mztc_enhancement", "set digital enhancement", "[value]", cliMztcEnhancement), + CLI_COMMAND_DEF("mztc_palette", "set color palette", "[palette]", cliMztcPalette), + CLI_COMMAND_DEF("mztc_preset", "apply a purpose preset", "[preset]", cliMztcPreset), + CLI_COMMAND_DEF("mztc_reconnect", "force a reconnect to the camera", NULL, cliMztcReconnect), + CLI_COMMAND_DEF("mztc_save", "save the current settings to the camera flash", NULL, cliMztcSave), + CLI_COMMAND_DEF("mztc_vignetting", "run one vignetting correction", NULL, cliMztcVignetting), + CLI_COMMAND_DEF("mztc_zoom", "set zoom level", "[level]", cliMztcZoom), +#endif CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), #ifdef USE_MOTOR_SRXL2 @@ -5168,6 +5462,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), +#ifdef USE_MZTC +#endif }; static void cliHelp(char *cmdline) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 9ba5a8e442c..3d44d3aceb3 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -91,6 +91,8 @@ #include "fc/firmware_update.h" #include "fc/stats.h" +#include "io/mztc_camera.h" + #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -602,6 +604,10 @@ void init(void) // to set the boxes up mspFcInit(); +#ifdef USE_MZTC + mztcInit(); +#endif + cliInit(serialConfig()); failsafeInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d00984f2d85..e410d419804 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -103,6 +103,8 @@ #include "io/serial_4way.h" #include "io/vtx.h" #include "io/vtx_string.h" +#include "io/mztc_camera.h" +#include "msp/msp_mztc.h" #include "io/gps_private.h" //for MSP_SIMULATOR #include "io/headtracker_msp.h" @@ -2018,6 +2020,43 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; +#ifdef USE_MZTC + // MassZero Thermal Camera MSP V2 output commands. + // Every field is written individually so the wire layout is fixed by this + // code. Compiler struct padding never reaches the wire. See msp_mztc.h. + case MSP2_MZTC_CONFIG: + { + const mztcConfig_t *cfg = mztcConfig(); + + sbufWriteU8(dst, cfg->preset); + sbufWriteU8(dst, cfg->palette_mode); + sbufWriteU8(dst, cfg->auto_shutter); + sbufWriteU8(dst, cfg->digital_enhancement); + sbufWriteU8(dst, cfg->spatial_denoise); + sbufWriteU8(dst, cfg->temporal_denoise); + sbufWriteU8(dst, cfg->brightness); + sbufWriteU8(dst, cfg->contrast); + sbufWriteU8(dst, cfg->zoom_level); + sbufWriteU8(dst, cfg->mirror_mode); + sbufWriteU8(dst, cfg->ffc_interval); + } + break; + + case MSP2_MZTC_STATUS: + { + const mztcStatus_t *status = mztcGetStatus(); + + sbufWriteU8(dst, status->status); + sbufWriteU8(dst, status->preset); + sbufWriteU8(dst, status->connected ? 1 : 0); + sbufWriteU8(dst, status->connection_quality); + sbufWriteU16(dst, status->last_calibration); + sbufWriteU8(dst, status->error_flags); + } + break; + +#endif + default: return false; } @@ -4099,6 +4138,119 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; +#ifdef USE_MZTC + // MassZero Thermal Camera MSP V2 input commands. + // Each field is read individually and the whole request is validated + // before any of it is applied. A rejected request leaves the running + // configuration untouched. See msp_mztc.h for the payload layouts. + case MSP2_SET_MZTC_CONFIG: + if (dataSize == MSP2_MZTC_CONFIG_PAYLOAD_SIZE) { + mztcConfig_t candidate; + + candidate.preset = sbufReadU8(src); + candidate.palette_mode = sbufReadU8(src); + candidate.auto_shutter = sbufReadU8(src); + candidate.digital_enhancement = sbufReadU8(src); + candidate.spatial_denoise = sbufReadU8(src); + candidate.temporal_denoise = sbufReadU8(src); + candidate.brightness = sbufReadU8(src); + candidate.contrast = sbufReadU8(src); + candidate.zoom_level = sbufReadU8(src); + candidate.mirror_mode = sbufReadU8(src); + candidate.ffc_interval = sbufReadU8(src); + + // Every enum and percentage is range checked here, so a value the + // CLI would reject cannot be smuggled in over MSP. + if (!mztcConfigIsValid(&candidate)) { + return MSP_RESULT_ERROR; + } + + *mztcConfigMutable() = candidate; + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_PRESET: + if (dataSize == 1) { + if (!mztcSetPreset((mztcPreset_e)sbufReadU8(src))) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_PALETTE: + if (dataSize == 1) { + if (!mztcSetPalette((mztcPaletteMode_e)sbufReadU8(src))) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_ZOOM: + if (dataSize == 1) { + if (!mztcSetZoom((mztcZoomLevel_e)sbufReadU8(src))) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_SHUTTER: + // A manual shutter cycle is this camera's flat field correction, so + // this is the same operation the CLI exposes as mztc_calibrate. + if (dataSize <= 1) { + if (!mztcTriggerCalibration()) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_IMAGE_PARAMS: + if (dataSize == MSP2_SET_MZTC_IMAGE_PARAMS_PAYLOAD_SIZE) { + const uint8_t brightness = sbufReadU8(src); + const uint8_t contrast = sbufReadU8(src); + const uint8_t enhancement = sbufReadU8(src); + if (!mztcSetImageParams(brightness, contrast, enhancement)) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_CORRECTION: + if (dataSize == MSP2_SET_MZTC_CORRECTION_PAYLOAD_SIZE) { + const uint8_t spatial = sbufReadU8(src); + const uint8_t temporal = sbufReadU8(src); + if (!mztcSetDenoising(spatial, temporal)) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP2_SET_MZTC_VIGNETTING: + // An action, not a setting. The lens has to be pointed at a uniform + // surface before this is worth running. + if (dataSize <= 1) { + if (!mztcTriggerVignettingCorrection()) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + case MSP2_INAV_SET_CRUISE_HEADING: // Set heading while Cruise / Course Hold is active. // Payload: I32 heading_centidegrees (0–35999) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 742c90f4510..2a3db195639 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -35,6 +35,10 @@ #include "io/osd.h" +#ifdef USE_MZTC +#include "io/mztc_camera.h" +#endif + #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -120,6 +124,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXTERRAINAGLHOLD, .boxName = "TERRAIN AGL HOLD", .permanentId = 70 }, { .boxId = BOXINFLIGHTMENU, .boxName = "IN FLIGHT MENU", .permanentId = 71 }, { .boxId = BOXTHRUSTREVERSE, .boxName = "THRUST REVERSE", .permanentId = 72 }, + { .boxId = BOXMZTCCALIBRATE, .boxName = "THERMAL CALIBRATE", .permanentId = 73 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -268,6 +273,15 @@ void initActiveBoxIds(void) } } +#ifdef USE_MZTC + // Only offered when the camera has a UART assigned in the Ports tab. The + // flat field correction it triggers uses the camera's internal shutter as + // its reference, so the scene in front of the lens does not matter. + if (mztcIsEnabled()) { + ADD_ACTIVE_BOX(BOXMZTCCALIBRATE); + } +#endif + #ifdef USE_MR_BRAKING_MODE if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || platformTypeConfigured(PLATFORM_MULTIROTOR)) { ADD_ACTIVE_BOX(BOXBRAKING); @@ -521,6 +535,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) * showed as off in the Configurator while the driver was acting on it. */ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTHRUSTREVERSE)), BOXTHRUSTREVERSE); #endif +#ifdef USE_MZTC + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMZTCCALIBRATE)), BOXMZTCCALIBRATE); +#endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 31143e92706..e8c850c1c40 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -66,6 +66,7 @@ #include "io/osd.h" #include "io/serial.h" #include "io/rcdevice_cam.h" +#include "io/mztc_camera.h" #include "io/osd_joystick.h" #include "io/smartport_master.h" #include "io/crsf_sensor.h" @@ -716,6 +717,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, #endif +#ifdef USE_MZTC + [TASK_MZTC_CAMERA] = { + .taskName = "MZTC", + .taskFunc = mztcUpdate, + .desiredPeriod = TASK_PERIOD_HZ(10), + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif #ifdef USE_CMS [TASK_CMS] = { diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 99d884cbce3..f340a5faea8 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -41,6 +41,10 @@ #include "fc/rc_curves.h" #include "fc/settings.h" +#ifdef USE_MZTC +#include "io/mztc_camera.h" +#endif + #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -278,6 +282,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_MZTC_ZOOM, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} }, { .adjustmentFunction = ADJUSTMENT_TPA, .mode = ADJUSTMENT_MODE_STEP, @@ -592,6 +600,20 @@ static void applyStepAdjustment(controlConfig_t *controlConfig, uint8_t adjustme } } break; +#endif +#ifdef USE_MZTC + case ADJUSTMENT_MZTC_ZOOM: + { + // Step through 1x, 2x, 4x and 8x. mztcSetZoom writes the camera + // and stores the level together, so the switch position and the + // saved setting still agree after a reconnect. + const int current = mztcConfig()->zoom_level; + const int wanted = constrain(current + delta, MZTC_ZOOM_1X, MZTC_ZOOM_8X); + if (wanted != current && mztcSetZoom((mztcZoomLevel_e)wanted)) { + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MZTC_ZOOM, wanted); + } + } + break; #endif case ADJUSTMENT_TPA: applyAdjustmentU8(ADJUSTMENT_TPA, &controlConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 684cc8bf148..03c8eaf73dd 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -86,6 +86,7 @@ typedef enum { ADJUSTMENT_FW_LEVEL_TRIM = 58, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX = 59, ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE = 60, + ADJUSTMENT_MZTC_ZOOM = 61, ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index a49733bfbe3..1db9c1e7a44 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -89,6 +89,7 @@ typedef enum { BOXTERRAINAGLHOLD = 61, BOXINFLIGHTMENU = 62, BOXTHRUSTREVERSE = 63, + BOXMZTCCALIBRATE = 64, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 91765fa2eb4..2aa85a09e7e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -127,6 +127,21 @@ tables: - name: vtx_frequency_groups values: ["FREQUENCYGROUP_5G8", "FREQUENCYGROUP_2G4", "FREQUENCYGROUP_1G3"] enum: vtxFrequencyGroups_e + - name: mztc_presets + values: ["CUSTOM", "GENERAL", "FIRE", "SEARCH", "SURVEILLANCE", "INSPECTION", "MARITIME"] + enum: mztcPreset_e + - name: mztc_palettes + values: ["WHITE_HOT", "BLACK_HOT", "FUSION_1", "RAINBOW", "FUSION_2", "IRON_RED_1", "IRON_RED_2", "SEPIA", "COLOR_1", "COLOR_2", "ICE_FIRE", "RAIN", "GREEN_HOT", "RED_HOT"] + enum: mztcPaletteMode_e + - name: mztc_shutter_modes + values: ["TEMP_ONLY", "TIME_ONLY", "TIME_AND_TEMP"] + enum: mztcShutterMode_e + - name: mztc_zoom_levels + values: ["1X", "2X", "4X", "8X"] + enum: mztcZoomLevel_e + - name: mztc_mirror_modes + values: ["NONE", "HORIZONTAL", "VERTICAL", "CENTRAL"] + enum: mztcMirrorMode_e - name: filter_type values: ["PT1", "BIQUAD"] - name: filter_type_full @@ -248,6 +263,11 @@ constants: MAX_CONTROL_PROFILE_COUNT: 3 MAX_BATTERY_PROFILE_COUNT: 3 + MZTC_MIN_FFC_INTERVAL: 1 + MZTC_MAX_FFC_INTERVAL: 60 + MZTC_MIN_PERCENT: 0 + MZTC_MAX_PERCENT: 100 + groups: - name: PG_GYRO_CONFIG @@ -4797,6 +4817,83 @@ groups: field: roll_ratio min: 0 max: 5 + - name: PG_MZTC_CAMERA_CONFIG + type: mztcConfig_t + headers: ["config/mztc_camera.h"] + condition: USE_MZTC + members: + - name: mztc_preset + description: "Purpose preset for the MassZero Thermal Camera. Selecting one writes the palette, brightness, contrast, enhancement, denoise, shutter mode and correction interval. CUSTOM writes nothing and keeps hand-tuned values" + type: uint8_t + default_value: "CUSTOM" + field: preset + table: mztc_presets + - name: mztc_palette_mode + description: "Color palette mode" + type: uint8_t + default_value: "WHITE_HOT" + field: palette_mode + table: mztc_palettes + - name: mztc_auto_shutter + description: "Auto shutter calibration mode" + type: uint8_t + default_value: "TIME_AND_TEMP" + field: auto_shutter + table: mztc_shutter_modes + - name: mztc_digital_enhancement + description: "Digital enhancement level (0-100)" + type: uint8_t + default_value: 50 + field: digital_enhancement + min: MZTC_MIN_PERCENT + max: MZTC_MAX_PERCENT + - name: mztc_spatial_denoise + description: "Spatial denoising level (0-100)" + type: uint8_t + default_value: 50 + field: spatial_denoise + min: MZTC_MIN_PERCENT + max: MZTC_MAX_PERCENT + - name: mztc_temporal_denoise + description: "Temporal denoising level (0-100)" + type: uint8_t + default_value: 50 + field: temporal_denoise + min: MZTC_MIN_PERCENT + max: MZTC_MAX_PERCENT + - name: mztc_brightness + description: "Image brightness (0-100)" + type: uint8_t + default_value: 50 + field: brightness + min: MZTC_MIN_PERCENT + max: MZTC_MAX_PERCENT + - name: mztc_contrast + description: "Image contrast (0-100)" + type: uint8_t + default_value: 50 + field: contrast + min: MZTC_MIN_PERCENT + max: MZTC_MAX_PERCENT + - name: mztc_zoom_level + description: "Digital zoom level" + type: uint8_t + default_value: "1X" + field: zoom_level + table: mztc_zoom_levels + - name: mztc_mirror_mode + description: "Image mirroring mode" + type: uint8_t + default_value: "NONE" + field: mirror_mode + table: mztc_mirror_modes + - name: mztc_ffc_interval + description: "Automatic shutter interval in minutes. The camera runs the schedule itself. Set mztc_auto_shutter to TEMP_ONLY to stop it correcting on a timer." + type: uint8_t + default_value: 5 + field: ffc_interval + min: MZTC_MIN_FFC_INTERVAL + max: MZTC_MAX_FFC_INTERVAL - name: PG_GEOZONE_CONFIG type: geozone_config_t headers: ["navigation/navigation.h"] diff --git a/src/main/io/mztc_camera.c b/src/main/io/mztc_camera.c new file mode 100644 index 00000000000..5d7168a5bf7 --- /dev/null +++ b/src/main/io/mztc_camera.c @@ -0,0 +1,987 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" +#include "build/build_config.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "fc/settings.h" + +#include "io/serial.h" +#include "io/mztc_camera.h" +#include "fc/rc_modes.h" + +#ifdef USE_MZTC + +// Flag definitions +#define MZTC_FLAG_WRITE 0x00 +#define MZTC_FLAG_READ 0x01 +#define MZTC_FLAG_SUCCESS 0x03 +#define MZTC_FLAG_ERROR 0x04 + +// Error codes reported by the camera in the first payload byte of an error reply +#define MZTC_ERR_NO_COMMAND 0x00 +#define MZTC_ERR_THRESHOLD 0x01 + +// Camera commands, as class and subclass pairs +#define MZTC_CLASS_DEVICE 0x74 +#define MZTC_CLASS_DISPLAY 0x70 +#define MZTC_CLASS_IMAGE 0x78 +#define MZTC_CLASS_SHUTTER 0x7C + +#define MZTC_SUB_READ_MODEL 0x02 +#define MZTC_SUB_READ_FPGA_VER 0x03 +#define MZTC_SUB_READ_SW_VER 0x05 +#define MZTC_SUB_RESTORE_DEFAULTS 0x0F +#define MZTC_SUB_SAVE_CONFIG 0x10 + +#define MZTC_SUB_MANUAL_SHUTTER 0x02 +#define MZTC_SUB_AUTO_SHUTTER 0x04 +#define MZTC_SUB_SHUTTER_INTERVAL 0x05 +#define MZTC_SUB_VIGNETTING 0x0C +#define MZTC_SUB_INIT_STATUS 0x14 + +// The camera answers the 0x7C/0x14 initialization status request on a +// different address than it was asked on. The manual specifies the reply as +// class 0x7D subclass 0x06. +#define MZTC_CLASS_INIT_STATUS_REPLY 0x7D +#define MZTC_SUB_INIT_STATUS_REPLY 0x06 + +// Vignetting correction takes a fixed data byte +#define MZTC_VIGNETTING_TRIGGER 0x02 + +#define MZTC_SUB_BRIGHTNESS 0x02 +#define MZTC_SUB_CONTRAST 0x03 +#define MZTC_SUB_DIGITAL_ENHANCE 0x10 +#define MZTC_SUB_SPATIAL_DENOISE 0x15 +#define MZTC_SUB_TEMPORAL_DENOISE 0x16 +#define MZTC_SUB_PSEUDO_COLOR 0x20 + +#define MZTC_SUB_IMAGE_MIRROR 0x11 +#define MZTC_SUB_ZOOM 0x12 + +// The MZTC_* limits in config/mztc_camera.h and the constants block in +// settings.yaml describe the same ranges to different consumers. The C code +// validates against the first. The CLI and the generated docs use the second. +// These assertions turn any divergence into a build failure. Without them the +// CLI could reject a value that MSP still accepts. +STATIC_ASSERT(MZTC_MIN_FFC_INTERVAL == SETTING_MZTC_FFC_INTERVAL_MIN, mztc_ffc_interval_min_mismatch); +STATIC_ASSERT(MZTC_MAX_FFC_INTERVAL == SETTING_MZTC_FFC_INTERVAL_MAX, mztc_ffc_interval_max_mismatch); +STATIC_ASSERT(MZTC_MIN_PERCENT == SETTING_MZTC_BRIGHTNESS_MIN, mztc_percent_min_mismatch); +STATIC_ASSERT(MZTC_MAX_PERCENT == SETTING_MZTC_BRIGHTNESS_MAX, mztc_percent_max_mismatch); + +// Connection management timings +#define MZTC_PORT_RETRY_MS 1000 // Between attempts to open the port +#define MZTC_PROBE_INTERVAL_MS 500 // Between identity probes +#define MZTC_PROBE_ATTEMPTS 6 // Reopen the port after this many unanswered probes +#define MZTC_RX_TIMEOUT_MS 3000 // No valid reply for this long means the link is down + +// Connection quality is the percentage of probes answered over a sliding window +#define MZTC_QUALITY_WINDOW 8 + +// Internal state +static mztcStatus_t mztcStatus; +static serialPort_t *mztcSerialPort = NULL; +static timeMs_t mztcLastUpdateTime = 0; +static timeMs_t mztcLastPortRetry = 0; +static timeMs_t mztcLastProbeTime = 0; +static timeMs_t mztcLastCalibrationTime = 0; +static timeMs_t mztcLastValidResponse = 0; +static bool mztcInitialized = false; +static uint8_t mztcProbeAttempts = 0; + +// Sliding window of probe outcomes, used for connection_quality +static uint8_t mztcProbesSent = 0; +static uint8_t mztcProbesAnswered = 0; + +// Set when the camera first answers, consumed by the task so that the +// configuration burst never runs in interrupt context. +static bool mztcConfigurationPending = false; +// Previous state of the THERMAL CALIBRATE switch. The correction runs on the +// rising edge only, so holding the switch does not fire it every task tick. +static bool mztcCalibrateBoxWasActive = false; +// The preset the stored settings currently reflect. Any path that changes +// mztc_preset is noticed by the task and applied, so "set mztc_preset" behaves +// the same as the mztc_preset CLI command and MSP2_SET_MZTC_PRESET. +static uint8_t mztcLastAppliedPreset = MZTC_PRESET_CUSTOM; + +// Device identity, filled in from the model and version replies +static uint8_t mztcDeviceModel[MZTC_MAX_DATA_LEN]; +static uint8_t mztcDeviceModelLen = 0; + +// Receive framing state. The parser is length driven so that a 0xF0 or 0xFF +// byte inside a payload cannot split or truncate a packet. +static uint8_t mztcRxBuffer[MZTC_MAX_PACKET_LEN]; +static uint8_t mztcRxLen = 0; +static uint8_t mztcRxExpected = 0; + +// Parameter group for MassZero Thermal Camera configuration. +// +// The defaults come from the SETTING_*_DEFAULT macros that the settings +// generator emits from settings.yaml. The CLI defaults and the fresh-EEPROM +// defaults cannot drift apart. +// Version 2. Version 1 dropped the five unused RC channel fields, the +// temperature settings and the crosshair flag, and widened last_calibration. +// Version 2 replaces the inert mode field with preset and removes update_rate, +// which shifts every field after the first byte. An older record read at the +// new offsets would apply garbage to real camera settings, so the bump makes +// it fall back to defaults instead. +PG_REGISTER_WITH_RESET_TEMPLATE(mztcConfig_t, mztcConfig, PG_MZTC_CAMERA_CONFIG, 2); + +PG_RESET_TEMPLATE(mztcConfig_t, mztcConfig, + .preset = SETTING_MZTC_PRESET_DEFAULT, + .palette_mode = SETTING_MZTC_PALETTE_MODE_DEFAULT, + .auto_shutter = SETTING_MZTC_AUTO_SHUTTER_DEFAULT, + .digital_enhancement = SETTING_MZTC_DIGITAL_ENHANCEMENT_DEFAULT, + .spatial_denoise = SETTING_MZTC_SPATIAL_DENOISE_DEFAULT, + .temporal_denoise = SETTING_MZTC_TEMPORAL_DENOISE_DEFAULT, + .brightness = SETTING_MZTC_BRIGHTNESS_DEFAULT, + .contrast = SETTING_MZTC_CONTRAST_DEFAULT, + .zoom_level = SETTING_MZTC_ZOOM_LEVEL_DEFAULT, + .mirror_mode = SETTING_MZTC_MIRROR_MODE_DEFAULT, + .ffc_interval = SETTING_MZTC_FFC_INTERVAL_DEFAULT, +); + +// Forward declarations +STATIC_UNIT_TESTED void mztcSerialReceiveCallback(uint16_t c, void *rxCallbackData); +static bool mztcSendPacket(uint8_t class_cmd, uint8_t subclass_cmd, uint8_t flags, const uint8_t *data, uint8_t data_len); +STATIC_UNIT_TESTED void mztcHandlePacket(const uint8_t *packet, uint8_t len); +static void mztcCheckCalibration(void); +STATIC_UNIT_TESTED void mztcSendConfiguration(void); +static void mztcClosePort(uint8_t errorFlag); +static void mztcSendProbe(void); + +/* + * Wire framing + */ + +// Build a complete packet into out. The caller must supply at least +// MZTC_MAX_PACKET_LEN bytes. Returns the number of bytes written, or 0 if the +// request is invalid. +uint8_t mztcBuildPacket(uint8_t *out, uint8_t class_cmd, uint8_t subclass_cmd, + uint8_t flags, const uint8_t *data, uint8_t data_len) +{ + if (!out || data_len > MZTC_MAX_DATA_LEN || (data_len > 0 && !data)) { + return 0; + } + + uint8_t i = 0; + out[i++] = MZTC_PACKET_BEGIN; + out[i++] = (uint8_t)(data_len + MZTC_SIZE_FIELD_OFFSET); + out[i++] = MZTC_DEVICE_ADDR; + out[i++] = class_cmd; + out[i++] = subclass_cmd; + out[i++] = flags; + + uint8_t checksum = MZTC_DEVICE_ADDR + class_cmd + subclass_cmd + flags; + for (uint8_t d = 0; d < data_len; d++) { + out[i++] = data[d]; + checksum += data[d]; + } + + out[i++] = checksum; + out[i++] = MZTC_PACKET_END; + + return i; +} + +// Validate a fully received packet: markers, declared length and checksum. +bool mztcPacketIsValid(const uint8_t *packet, uint8_t len) +{ + if (!packet || len < MZTC_MIN_PACKET_LEN || len > MZTC_MAX_PACKET_LEN) { + return false; + } + + if (packet[0] != MZTC_PACKET_BEGIN || packet[len - 1] != MZTC_PACKET_END) { + return false; + } + + const uint8_t declaredSize = packet[1]; + if (declaredSize < MZTC_SIZE_FIELD_OFFSET || (uint16_t)declaredSize + 4u != (uint16_t)len) { + return false; + } + + if (packet[2] != MZTC_DEVICE_ADDR) { + return false; + } + + // The checksum covers address, class, subclass, flags and payload. + uint8_t checksum = 0; + for (uint8_t i = 2; i < (uint8_t)(len - 2); i++) { + checksum += packet[i]; + } + + return checksum == packet[len - 2]; +} + +// Serial receive callback. Accumulates one packet at a time using the declared +// length. Payload bytes that happen to equal the begin or end markers cannot +// desynchronise the parser. +STATIC_UNIT_TESTED void mztcSerialReceiveCallback(uint16_t c, void *rxCallbackData) +{ + UNUSED(rxCallbackData); + + const uint8_t byte = (uint8_t)c; + + if (mztcRxLen == 0) { + // Waiting for a start of frame + if (byte != MZTC_PACKET_BEGIN) { + return; + } + mztcRxBuffer[mztcRxLen++] = byte; + mztcRxExpected = 0; + return; + } + + if (mztcRxLen == 1) { + // The size byte determines the length of the rest of the frame. + const uint16_t total = (uint16_t)byte + 4u; + if (byte < MZTC_SIZE_FIELD_OFFSET || total > MZTC_MAX_PACKET_LEN) { + // Bogus length. Resync on the next begin marker. + mztcRxLen = 0; + return; + } + mztcRxExpected = (uint8_t)total; + mztcRxBuffer[mztcRxLen++] = byte; + return; + } + + mztcRxBuffer[mztcRxLen++] = byte; + + if (mztcRxLen >= mztcRxExpected) { + if (mztcPacketIsValid(mztcRxBuffer, mztcRxLen)) { + mztcHandlePacket(mztcRxBuffer, mztcRxLen); + } + mztcRxLen = 0; + mztcRxExpected = 0; + } +} + +// Send a packet to the camera +static bool mztcSendPacket(uint8_t class_cmd, uint8_t subclass_cmd, uint8_t flags, const uint8_t *data, uint8_t data_len) +{ + if (!mztcSerialPort) { + return false; + } + + uint8_t packet[MZTC_MAX_PACKET_LEN]; + const uint8_t len = mztcBuildPacket(packet, class_cmd, subclass_cmd, flags, data, data_len); + if (len == 0) { + return false; + } + + serialWriteBufShim(mztcSerialPort, packet, len); + return true; +} + +/* + * Response handling + */ + +// Decode the payload of a successful reply. payloadLen is the payload length, +// which the caller derived from the validated frame. +static void mztcDecodeSuccess(uint8_t class_cmd, uint8_t subclass_cmd, const uint8_t *payload, uint8_t payloadLen) +{ + switch (class_cmd) { + case MZTC_CLASS_DEVICE: + switch (subclass_cmd) { + case MZTC_SUB_READ_MODEL: + case MZTC_SUB_READ_FPGA_VER: + case MZTC_SUB_READ_SW_VER: + if (payloadLen > 0) { + mztcDeviceModelLen = MIN(payloadLen, (uint8_t)sizeof(mztcDeviceModel)); + memcpy(mztcDeviceModel, payload, mztcDeviceModelLen); + } + break; + default: + break; + } + break; + + case MZTC_CLASS_INIT_STATUS_REPLY: + if (subclass_cmd == MZTC_SUB_INIT_STATUS_REPLY && payloadLen >= 1) { + // 0x00 is the logo loading stage. 0x01 is the image output stage. + if (payload[0] != 0) { + if (mztcStatus.status == MZTC_STATUS_INITIALIZING) { + mztcStatus.status = MZTC_STATUS_READY; + } + } else { + mztcStatus.status = MZTC_STATUS_INITIALIZING; + } + } + break; + + case MZTC_CLASS_SHUTTER: + switch (subclass_cmd) { + case MZTC_SUB_MANUAL_SHUTTER: + // The shutter cycle finished. The calibration clock restarts. + mztcLastCalibrationTime = millis(); + mztcStatus.last_calibration = 0; + mztcStatus.error_flags &= (uint8_t)~MZTC_ERROR_CALIBRATION; + if (mztcStatus.status == MZTC_STATUS_CALIBRATING) { + mztcStatus.status = MZTC_STATUS_READY; + } + break; + + default: + break; + } + break; + + default: + // Image and display commands acknowledge without a payload we consume. + break; + } +} + +// Handle one validated packet +STATIC_UNIT_TESTED void mztcHandlePacket(const uint8_t *packet, uint8_t len) +{ + const uint8_t class_cmd = packet[3]; + const uint8_t subclass_cmd = packet[4]; + const uint8_t flags = packet[5]; + const uint8_t *payload = &packet[6]; + const uint8_t payloadLen = (uint8_t)(len - MZTC_PACKET_OVERHEAD); + + mztcLastValidResponse = millis(); + + if (class_cmd == MZTC_CLASS_DEVICE && subclass_cmd == MZTC_SUB_READ_MODEL && + mztcProbesAnswered < mztcProbesSent) { + mztcProbesAnswered++; + } + + switch (flags) { + case MZTC_FLAG_SUCCESS: + mztcStatus.error_flags &= (uint8_t)~(MZTC_ERROR_COMMUNICATION | MZTC_ERROR_TIMEOUT); + mztcDecodeSuccess(class_cmd, subclass_cmd, payload, payloadLen); + break; + + case MZTC_FLAG_ERROR: + mztcStatus.error_flags |= MZTC_ERROR_COMMUNICATION; + if (payloadLen >= 1) { + switch (payload[0]) { + case MZTC_ERR_NO_COMMAND: + debug[1] = 0x01; + break; + case MZTC_ERR_THRESHOLD: + debug[1] = 0x02; + break; + default: + debug[1] = 0x03; + break; + } + } + break; + + default: + // Not a reply we decode, but the link is clearly alive. + break; + } + + // The camera answered. The link is up regardless of which command it was. + // This runs in the serial receive interrupt. The configuration burst is + // deferred to the task. + if (!mztcStatus.connected) { + mztcStatus.connected = true; + mztcProbeAttempts = 0; + mztcConfigurationPending = true; + if (mztcStatus.status == MZTC_STATUS_INITIALIZING || mztcStatus.status == MZTC_STATUS_ERROR) { + mztcStatus.status = MZTC_STATUS_READY; + } + } +} + +/* + * Connection management + */ + +static void mztcResetLinkState(void) +{ + mztcRxLen = 0; + mztcRxExpected = 0; + mztcProbeAttempts = 0; + mztcProbesSent = 0; + mztcProbesAnswered = 0; + mztcConfigurationPending = false; + mztcCalibrateBoxWasActive = false; + mztcLastAppliedPreset = mztcConfig()->preset; + mztcDeviceModelLen = 0; + mztcStatus.connected = false; + mztcStatus.connection_quality = 0; +} + +static void mztcClosePort(uint8_t errorFlag) +{ + if (mztcSerialPort) { + closeSerialPort(mztcSerialPort); + mztcSerialPort = NULL; + } + mztcResetLinkState(); + mztcStatus.status = MZTC_STATUS_ERROR; + mztcStatus.error_flags |= errorFlag; +} + +// Ask the camera to identify itself. A reply is what promotes the link from +// "the port is open" to "a camera is present". +static void mztcSendProbe(void) +{ + mztcLastProbeTime = millis(); + if (mztcProbeAttempts < UINT8_MAX) { + mztcProbeAttempts++; + } + if (mztcProbesSent < MZTC_QUALITY_WINDOW) { + mztcProbesSent++; + } else { + // Slide the window so quality tracks recent traffic. Without this it + // would average everything since the port opened. + mztcProbesAnswered -= mztcProbesAnswered / MZTC_QUALITY_WINDOW; + } + mztcSendPacket(MZTC_CLASS_DEVICE, MZTC_SUB_READ_MODEL, MZTC_FLAG_READ, NULL, 0); +} + +static void mztcUpdateConnectionQuality(void) +{ + if (mztcProbesSent == 0) { + mztcStatus.connection_quality = 0; + return; + } + mztcStatus.connection_quality = (uint8_t)((mztcProbesAnswered * 100u) / mztcProbesSent); +} + +// Try to open the configured port. Opening it only means the UART is ours; it +// says nothing about whether a camera is attached. +static void mztcTryOpenPort(timeMs_t now) +{ + if ((now - mztcLastPortRetry) < MZTC_PORT_RETRY_MS) { + return; + } + mztcLastPortRetry = now; + + // The Ports tab owns both the port and its baud rate. No assignment means + // no camera, which is how the feature is turned on and off. + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MZTC_CAMERA); + if (portConfig == NULL) { + mztcStatus.status = MZTC_STATUS_OFFLINE; + return; + } + + mztcSerialPort = openSerialPort(portConfig->identifier, + FUNCTION_MZTC_CAMERA, + mztcSerialReceiveCallback, + NULL, + baudRates[portConfig->peripheral_baudrateIndex], + MODE_RXTX, + SERIAL_NOT_INVERTED); + + if (mztcSerialPort == NULL) { + mztcStatus.status = MZTC_STATUS_ERROR; + mztcStatus.error_flags |= MZTC_ERROR_COMMUNICATION; + return; + } + + mztcResetLinkState(); + mztcStatus.status = MZTC_STATUS_INITIALIZING; + mztcStatus.error_flags = 0; + mztcLastValidResponse = now; + mztcSendProbe(); +} + +/* + * Public API + */ + +void mztcInit(void) +{ + if (mztcInitialized) { + return; + } + + memset(&mztcStatus, 0, sizeof(mztcStatus)); + mztcStatus.status = MZTC_STATUS_OFFLINE; + mztcStatus.preset = mztcConfig()->preset; + mztcLastAppliedPreset = mztcConfig()->preset; + mztcStatus.connected = false; + + mztcInitialized = true; + + // The port is opened from the update loop so that a missing or busy UART + // does not stall init. + mztcStatus.status = MZTC_STATUS_INITIALIZING; + mztcLastUpdateTime = millis(); + mztcLastPortRetry = 0; + mztcLastCalibrationTime = millis(); +} + +void mztcUpdate(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!mztcInitialized) { + return; + } + + const timeMs_t now = millis(); + + if (mztcSerialPort == NULL) { + mztcTryOpenPort(now); + return; + } + + if (!mztcStatus.connected) { + // Still probing. Reopen the port after a bounded number of unanswered + // probes, in case the UART came up before the camera did. The receive + // timeout below does not apply here, because nothing has answered yet. + if ((now - mztcLastProbeTime) >= MZTC_PROBE_INTERVAL_MS) { + if (mztcProbeAttempts >= MZTC_PROBE_ATTEMPTS) { + mztcClosePort(MZTC_ERROR_COMMUNICATION); + return; + } + mztcSendProbe(); + } + mztcUpdateConnectionQuality(); + return; + } + + // An established link is only healthy while the camera keeps answering. + if ((now - mztcLastValidResponse) > MZTC_RX_TIMEOUT_MS) { + mztcClosePort(MZTC_ERROR_TIMEOUT); + return; + } + + if (mztcConfigurationPending) { + mztcConfigurationPending = false; + mztcSendConfiguration(); + } + + mztcLastUpdateTime = now; + + // A periodic probe both feeds the receive timeout and measures link quality. + if ((now - mztcLastProbeTime) >= MZTC_PROBE_INTERVAL_MS) { + mztcSendProbe(); + } + mztcUpdateConnectionQuality(); + + // A rising edge on the switch triggers one flat field correction. The + // camera closes its own shutter as the reference, so this is safe to fire + // at any time and in any attitude. + const bool calibrateBoxActive = IS_RC_MODE_ACTIVE(BOXMZTCCALIBRATE); + if (calibrateBoxActive && !mztcCalibrateBoxWasActive) { + mztcTriggerCalibration(); + } + mztcCalibrateBoxWasActive = calibrateBoxActive; + + // A preset written through the settings framework or "set mztc_preset" + // only stores a byte. Applying it here is what makes every path agree. + if (mztcConfig()->preset != mztcLastAppliedPreset) { + mztcSetPreset((mztcPreset_e)mztcConfig()->preset); + } + + mztcCheckCalibration(); + + mztcStatus.preset = mztcConfig()->preset; +} + +bool mztcIsConnected(void) +{ + return mztcStatus.connected && (mztcSerialPort != NULL); +} + +// The camera is enabled by assigning its function to a UART in the Ports tab. +bool mztcIsEnabled(void) +{ + return mztcInitialized && findSerialPortConfig(FUNCTION_MZTC_CAMERA) != NULL; +} + +mztcStatus_t* mztcGetStatus(void) +{ + return &mztcStatus; +} + +// Identity bytes from the camera model or version reply. +const uint8_t *mztcGetDeviceId(uint8_t *len) +{ + if (len) { + *len = mztcDeviceModelLen; + } + return mztcDeviceModelLen > 0 ? mztcDeviceModel : NULL; +} + +bool mztcTriggerCalibration(void) +{ + if (!mztcIsEnabled() || !mztcIsConnected()) { + return false; + } + + if (mztcSendPacket(MZTC_CLASS_SHUTTER, MZTC_SUB_MANUAL_SHUTTER, MZTC_FLAG_WRITE, NULL, 0)) { + mztcStatus.status = MZTC_STATUS_CALIBRATING; + mztcLastCalibrationTime = millis(); + return true; + } + + return false; +} + +// The settings a preset owns. Zoom and mirror are deliberately absent: zoom +// belongs to the pilot and mirror describes how the camera is mounted, so a +// preset that overwrote either would fight the operator. +typedef struct mztcPresetValues_s { + uint8_t palette_mode; + uint8_t brightness; + uint8_t contrast; + uint8_t digital_enhancement; + uint8_t spatial_denoise; + uint8_t temporal_denoise; + uint8_t auto_shutter; + uint8_t ffc_interval; +} mztcPresetValues_t; + +// Two constraints shape every row. Temporal denoising averages across frames, +// so on a moving airframe it smears targets and leaves trails. Spatial +// denoising trades noise for sharpness, and a person at search range is only a +// few pixels wide. Both stay low wherever small distant targets matter. +// +// Indexed by mztcPreset_e. MZTC_PRESET_CUSTOM has no row because it writes +// nothing. +static const mztcPresetValues_t mztcPresets[] = { + [MZTC_PRESET_GENERAL] = { + MZTC_PALETTE_WHITE_HOT, 50, 50, 50, 40, 20, MZTC_SHUTTER_TIME_AND_TEMP, 5 + }, + [MZTC_PRESET_FIRE] = { + // Low enhancement is the important value here. Enhancement lifts + // mid-tones, and flat mid-tones are what let an extreme spike dominate. + MZTC_PALETTE_IRON_RED_1, 45, 75, 25, 30, 15, MZTC_SHUTTER_TIME_AND_TEMP, 10 + }, + [MZTC_PRESET_SEARCH] = { + // A clothed body sits a few degrees over ambient, so enhancement goes + // high. Both denoise values drop to keep a few-pixel target alive. The + // short correction interval matters more than it looks, because a + // drifting sensor grows fixed-pattern blobs that read as false targets. + MZTC_PALETTE_WHITE_HOT, 55, 60, 80, 20, 10, MZTC_SHUTTER_TIME_AND_TEMP, 3 + }, + [MZTC_PRESET_SURVEILLANCE] = { + // The one case where temporal denoising earns its keep. Loiter and + // hover mean little frame to frame motion, so averaging cleans rather + // than smears. + MZTC_PALETTE_GREEN_HOT, 50, 55, 60, 45, 45, MZTC_SHUTTER_TIME_AND_TEMP, 15 + }, + [MZTC_PRESET_INSPECTION] = { + // Comparing one panel cell against its neighbour is a uniformity + // problem, hence the shortest correction interval in the set. + MZTC_PALETTE_RAINBOW, 50, 45, 70, 55, 35, MZTC_SHUTTER_TIME_AND_TEMP, 2 + }, + [MZTC_PRESET_MARITIME] = { + // A uniform cold background takes high contrast well. Enhancement + // stays moderate because raising it amplifies wave texture into + // clutter. + MZTC_PALETTE_BLACK_HOT, 50, 70, 45, 25, 15, MZTC_SHUTTER_TIME_AND_TEMP, 5 + }, +}; + +bool mztcSetPreset(mztcPreset_e preset) +{ + if (!mztcIsEnabled() || preset > MZTC_PRESET_MARITIME) { + return false; + } + + mztcConfig_t *cfg = mztcConfigMutable(); + cfg->preset = preset; + mztcStatus.preset = preset; + mztcLastAppliedPreset = preset; + + // Custom keeps whatever the user configured. + if (preset == MZTC_PRESET_CUSTOM) { + return true; + } + + const mztcPresetValues_t *v = &mztcPresets[preset]; + cfg->palette_mode = v->palette_mode; + cfg->brightness = v->brightness; + cfg->contrast = v->contrast; + cfg->digital_enhancement = v->digital_enhancement; + cfg->spatial_denoise = v->spatial_denoise; + cfg->temporal_denoise = v->temporal_denoise; + cfg->auto_shutter = v->auto_shutter; + cfg->ffc_interval = v->ffc_interval; + + // Push the whole set on the next task run rather than writing here, so the + // burst stays out of whatever context called this. + mztcConfigurationPending = true; + return true; +} + +bool mztcSetPalette(mztcPaletteMode_e palette) +{ + if (!mztcIsEnabled() || palette > MZTC_PALETTE_RED_HOT) { + return false; + } + + const uint8_t value = (uint8_t)palette; + if (mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_PSEUDO_COLOR, MZTC_FLAG_WRITE, &value, 1)) { + mztcConfigMutable()->palette_mode = value; + return true; + } + + return false; +} + +bool mztcSetZoom(mztcZoomLevel_e zoom) +{ + if (!mztcIsEnabled() || zoom > MZTC_ZOOM_8X) { + return false; + } + + const uint8_t value = (uint8_t)zoom; + if (mztcSendPacket(MZTC_CLASS_DISPLAY, MZTC_SUB_ZOOM, MZTC_FLAG_WRITE, &value, 1)) { + mztcConfigMutable()->zoom_level = value; + return true; + } + + return false; +} + +bool mztcSetImageParams(uint8_t brightness, uint8_t contrast, uint8_t enhancement) +{ + if (!mztcIsEnabled()) { + return false; + } + + if (brightness > MZTC_MAX_PERCENT || contrast > MZTC_MAX_PERCENT || enhancement > MZTC_MAX_PERCENT) { + return false; + } + + bool success = mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_BRIGHTNESS, MZTC_FLAG_WRITE, &brightness, 1); + success = mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_CONTRAST, MZTC_FLAG_WRITE, &contrast, 1) && success; + success = mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_DIGITAL_ENHANCE, MZTC_FLAG_WRITE, &enhancement, 1) && success; + + if (success) { + mztcConfigMutable()->brightness = brightness; + mztcConfigMutable()->contrast = contrast; + mztcConfigMutable()->digital_enhancement = enhancement; + } + + return success; +} + +bool mztcSetDenoising(uint8_t spatial, uint8_t temporal) +{ + if (!mztcIsEnabled()) { + return false; + } + + if (spatial > MZTC_MAX_PERCENT || temporal > MZTC_MAX_PERCENT) { + return false; + } + + bool success = mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_SPATIAL_DENOISE, MZTC_FLAG_WRITE, &spatial, 1); + success = mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_TEMPORAL_DENOISE, MZTC_FLAG_WRITE, &temporal, 1) && success; + + if (success) { + mztcConfigMutable()->spatial_denoise = spatial; + mztcConfigMutable()->temporal_denoise = temporal; + } + + return success; +} + +// Validate a candidate configuration in full. The MSP set handler uses this so +// that a rejected request changes nothing at all. +bool mztcConfigIsValid(const mztcConfig_t *cfg) +{ + if (!cfg) { + return false; + } + + if (cfg->preset > MZTC_PRESET_MARITIME || + cfg->palette_mode > MZTC_PALETTE_RED_HOT || + cfg->auto_shutter > MZTC_SHUTTER_TIME_AND_TEMP || + cfg->zoom_level > MZTC_ZOOM_8X || + cfg->mirror_mode > MZTC_MIRROR_CENTRAL) { + return false; + } + + if (cfg->ffc_interval < MZTC_MIN_FFC_INTERVAL || cfg->ffc_interval > MZTC_MAX_FFC_INTERVAL) { + return false; + } + + if (cfg->digital_enhancement > MZTC_MAX_PERCENT || cfg->spatial_denoise > MZTC_MAX_PERCENT || + cfg->temporal_denoise > MZTC_MAX_PERCENT || cfg->brightness > MZTC_MAX_PERCENT || + cfg->contrast > MZTC_MAX_PERCENT) { + return false; + } + + return true; +} + +// The camera runs its own shutter schedule from the interval we push to it +// with MZTC_SUB_SHUTTER_INTERVAL, so this only tracks elapsed time for the +// status and OSD surfaces. A host-side timer here would fight the camera. +static void mztcCheckCalibration(void) +{ + const uint32_t minutes = (millis() - mztcLastCalibrationTime) / (60u * 1000u); + + mztcStatus.last_calibration = (uint16_t)MIN(minutes, (uint32_t)UINT16_MAX); +} + +STATIC_UNIT_TESTED void mztcSendConfiguration(void) +{ + if (!mztcSerialPort) { + return; + } + + const mztcConfig_t *cfg = mztcConfig(); + + // The camera accepts 0x01 to 0x03 for the shutter mode and rejects 0x00 as + // out of range, so the zero-based setting is shifted before it goes out. + const uint8_t shutterMode = (uint8_t)(cfg->auto_shutter + MZTC_SHUTTER_WIRE_OFFSET); + mztcSendPacket(MZTC_CLASS_SHUTTER, MZTC_SUB_AUTO_SHUTTER, MZTC_FLAG_WRITE, &shutterMode, 1); + + // Two byte interval in minutes, high byte first. Everything in the allowed + // range fits in the low byte, so the high byte is always zero here. + const uint8_t interval[2] = { 0, cfg->ffc_interval }; + mztcSendPacket(MZTC_CLASS_SHUTTER, MZTC_SUB_SHUTTER_INTERVAL, MZTC_FLAG_WRITE, interval, 2); + + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_DIGITAL_ENHANCE, MZTC_FLAG_WRITE, &cfg->digital_enhancement, 1); + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_SPATIAL_DENOISE, MZTC_FLAG_WRITE, &cfg->spatial_denoise, 1); + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_TEMPORAL_DENOISE, MZTC_FLAG_WRITE, &cfg->temporal_denoise, 1); + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_BRIGHTNESS, MZTC_FLAG_WRITE, &cfg->brightness, 1); + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_CONTRAST, MZTC_FLAG_WRITE, &cfg->contrast, 1); + mztcSendPacket(MZTC_CLASS_IMAGE, MZTC_SUB_PSEUDO_COLOR, MZTC_FLAG_WRITE, &cfg->palette_mode, 1); + mztcSendPacket(MZTC_CLASS_DISPLAY, MZTC_SUB_ZOOM, MZTC_FLAG_WRITE, &cfg->zoom_level, 1); + mztcSendPacket(MZTC_CLASS_DISPLAY, MZTC_SUB_IMAGE_MIRROR, MZTC_FLAG_WRITE, &cfg->mirror_mode, 1); +} + +void mztcRequestReconnect(void) +{ + if (mztcSerialPort != NULL) { + closeSerialPort(mztcSerialPort); + mztcSerialPort = NULL; + } + mztcResetLinkState(); + mztcStatus.status = MZTC_STATUS_INITIALIZING; + mztcStatus.error_flags = 0; + mztcLastPortRetry = 0; // force an immediate retry in the update loop +} + +bool mztcSaveConfiguration(void) +{ + if (!mztcIsEnabled() || !mztcIsConnected()) { + return false; + } + + return mztcSendPacket(MZTC_CLASS_DEVICE, MZTC_SUB_SAVE_CONFIG, MZTC_FLAG_WRITE, NULL, 0); +} + +// Vignetting correction is a one-shot action, not a stored setting. The camera +// manual requires the lens to be pointed at a uniform surface first, so this is +// only ever run on request. +bool mztcTriggerVignettingCorrection(void) +{ + if (!mztcIsEnabled() || !mztcIsConnected()) { + return false; + } + + const uint8_t trigger = MZTC_VIGNETTING_TRIGGER; + return mztcSendPacket(MZTC_CLASS_SHUTTER, MZTC_SUB_VIGNETTING, MZTC_FLAG_WRITE, &trigger, 1); +} + +bool mztcRestoreDefaults(void) +{ + if (!mztcIsEnabled() || !mztcIsConnected()) { + return false; + } + + if (!mztcSendPacket(MZTC_CLASS_DEVICE, MZTC_SUB_RESTORE_DEFAULTS, MZTC_FLAG_WRITE, NULL, 0)) { + return false; + } + + // Mirror the camera-side reset in our own copy of the settings. + mztcConfig_t *cfg = mztcConfigMutable(); + cfg->brightness = SETTING_MZTC_BRIGHTNESS_DEFAULT; + cfg->contrast = SETTING_MZTC_CONTRAST_DEFAULT; + cfg->digital_enhancement = SETTING_MZTC_DIGITAL_ENHANCEMENT_DEFAULT; + cfg->spatial_denoise = SETTING_MZTC_SPATIAL_DENOISE_DEFAULT; + cfg->temporal_denoise = SETTING_MZTC_TEMPORAL_DENOISE_DEFAULT; + cfg->palette_mode = SETTING_MZTC_PALETTE_MODE_DEFAULT; + cfg->zoom_level = SETTING_MZTC_ZOOM_LEVEL_DEFAULT; + cfg->mirror_mode = SETTING_MZTC_MIRROR_MODE_DEFAULT; + cfg->auto_shutter = SETTING_MZTC_AUTO_SHUTTER_DEFAULT; + + return true; +} + + +#ifdef UNIT_TEST +/* + * Test hooks. + * + * The receive-path tests drive mztcSerialReceiveCallback() directly rather than + * opening a port, so they need a way to put the driver into the state a live + * link would have reached and to read back state the driver otherwise keeps to + * itself. + */ +void mztcTestReset(void) +{ + memset(&mztcStatus, 0, sizeof(mztcStatus)); + mztcRxLen = 0; + mztcRxExpected = 0; + mztcProbeAttempts = 0; + mztcProbesSent = 0; + mztcProbesAnswered = 0; + mztcConfigurationPending = false; + mztcDeviceModelLen = 0; + mztcInitialized = true; + mztcLastCalibrationTime = 0; + mztcLastValidResponse = 0; + + // Any non-null value. Nothing dereferences it, because serialWriteBufShim + // is stubbed in the test. + mztcSerialPort = (serialPort_t *)&mztcStatus; +} + +// Clear the initialised flag so a test can run the real mztcInit() boot path. +// Without this mztcInit() returns immediately and a boot test proves nothing. +void mztcTestForceReinit(void) +{ + mztcInitialized = false; +} + +void mztcTestSetStatus(uint8_t status) +{ + mztcStatus.status = status; +} + +void mztcTestSetLastCalibration(uint16_t minutes) +{ + mztcStatus.last_calibration = minutes; +} +#endif + +#endif // USE_MZTC diff --git a/src/main/io/mztc_camera.h b/src/main/io/mztc_camera.h new file mode 100644 index 00000000000..49dcfd839ea --- /dev/null +++ b/src/main/io/mztc_camera.h @@ -0,0 +1,35 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include "drivers/time.h" +#include "drivers/serial.h" +#include "config/mztc_camera.h" + +#ifdef USE_MZTC + +// The driver API lives in config/mztc_camera.h alongside the data model. That +// keeps the parameter group, the limits and the functions that enforce them in +// one place. This header exists for the callers that only want the driver. It +// adds the accessors that are outside the data model. + +// Identity bytes reported by the camera in its model or version reply. Returns +// NULL until the camera has answered a probe. len receives the byte count. +const uint8_t *mztcGetDeviceId(uint8_t *len); + +#endif // USE_MZTC diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 961cb1ed309..9daf85a2000 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -78,6 +78,7 @@ #include "io/vtx_string.h" #include "io/osd/custom_elements.h" +#include "io/osd/mztc_camera_osd.h" #include "fc/config.h" #include "fc/control_profile.h" @@ -4119,6 +4120,12 @@ static bool osdDrawSingleElement(uint8_t item) } #endif +#ifdef USE_MZTC + case OSD_MZTC_STATUS: + mztcOsdFormatStatus(buff, &elemAttr); + break; +#endif + default: return false; } @@ -4583,6 +4590,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_BLACKBOX] = OSD_POS(2, 10); #endif +#ifdef USE_MZTC + osdLayoutsConfig->item_pos[0][OSD_MZTC_STATUS] = OSD_POS(1, 4); +#endif + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 9eab77e4ba0..3f34a5de4ba 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -381,6 +381,7 @@ typedef enum { OSD_GPS_EXTRA_STATS, OSD_AUTO_SPEED, // 170 OSD_TERRAIN_AGL, // 171, + OSD_MZTC_STATUS, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/io/osd/mztc_camera_osd.c b/src/main/io/osd/mztc_camera_osd.c new file mode 100644 index 00000000000..765fc31daf7 --- /dev/null +++ b/src/main/io/osd/mztc_camera_osd.c @@ -0,0 +1,72 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/printf.h" +#include "common/utils.h" + +#include "drivers/display.h" + +#include "io/osd/mztc_camera_osd.h" +#include "io/mztc_camera.h" + +#ifdef USE_MZTC + +void mztcOsdFormatStatus(char *buff, textAttributes_t *attr) +{ + const mztcStatus_t *status = mztcGetStatus(); + const char *text; + + switch (status->status) { + case MZTC_STATUS_READY: + case MZTC_STATUS_CAPTURING: + text = "OK "; + break; + case MZTC_STATUS_INITIALIZING: + text = "INI"; + break; + case MZTC_STATUS_CALIBRATING: + text = "FFC"; + break; + case MZTC_STATUS_RECORDING: + text = "REC"; + break; + case MZTC_STATUS_ALERT: + text = "ALT"; + break; + case MZTC_STATUS_ERROR: + text = "ERR"; + break; + case MZTC_STATUS_OFFLINE: + default: + text = "OFF"; + break; + } + + tfp_sprintf(buff, "IR %s", text); + + if (status->status == MZTC_STATUS_ERROR || status->status == MZTC_STATUS_OFFLINE) { + TEXT_ATTRIBUTES_ADD_BLINK(*attr); + } +} + +#endif // USE_MZTC diff --git a/src/main/io/osd/mztc_camera_osd.h b/src/main/io/osd/mztc_camera_osd.h new file mode 100644 index 00000000000..0360ba16280 --- /dev/null +++ b/src/main/io/osd/mztc_camera_osd.h @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include + +#include "drivers/display.h" + +#ifdef USE_MZTC + +/* + * MassZero Thermal Camera OSD element. + * + * Position and visibility come from INAV's own OSD layout configuration. The + * item is OSD_MZTC_STATUS in osd_items_e. This module owns no parameter group + * and schedules no updates of its own. It only formats the text that + * osdDrawSingleElement() then draws. + * + * The function writes a NUL-terminated string into buff. The caller must + * supply at least MZTC_OSD_ELEMENT_LENGTH bytes. It sets *attr for blink when + * the link needs attention. + */ + +#define MZTC_OSD_ELEMENT_LENGTH 12 + +void mztcOsdFormatStatus(char *buff, textAttributes_t *attr); + +#endif // USE_MZTC diff --git a/src/main/io/serial.h b/src/main/io/serial.h index f9ec6dbb4fd..cc29acb108c 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -59,8 +59,9 @@ typedef enum { FUNCTION_MSP_OSD = (1 << 25), // 33554432 FUNCTION_GIMBAL = (1 << 26), // 67108864 FUNCTION_GIMBAL_HEADTRACKER = (1 << 27), // 134217728 - /* 28 is left free: the Configurator already assigns it to the MassZero thermal - * camera, whose firmware side is not on this branch yet. */ +#ifdef USE_MZTC + FUNCTION_MZTC_CAMERA = (1 << 28), // 268435456 - MassZero Thermal Camera +#endif FUNCTION_ESC_SRXL2 = (1 << 29), // 536870912: Spektrum Smart ESC (Smart Throttle) } serialPortFunction_e; diff --git a/src/main/msp/msp_mztc.h b/src/main/msp/msp_mztc.h new file mode 100644 index 00000000000..f0fb3309741 --- /dev/null +++ b/src/main/msp/msp_mztc.h @@ -0,0 +1,101 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include + +#include "config/mztc_camera.h" + +#ifdef USE_MZTC + +/* + * MassZero Thermal Camera MSP V2 commands. + * + * These live in INAV's own 0x2000-0x2FFF range. The 0x3000 block belongs to + * the Betaflight compatibility commands. MSP2_BETAFLIGHT_BIND is 0x3000. + * MSP2_RX_BIND is 0x3001. That block is out of reach here. + * + * Every command listed below is handled in fc_msp.c. There are no reserved or + * aliased identifiers: an ID exists only if something answers it. + * + * All payloads are little endian. Every field is read and written one at a + * time with the sbuf helpers. No C struct is cast over the stream buffer, which + * keeps compiler padding and alignment off the wire. + */ + +// Out (flight controller to host) +#define MSP2_MZTC_CONFIG 0x2240 +#define MSP2_MZTC_STATUS 0x2241 + +// In (host to flight controller) +#define MSP2_SET_MZTC_CONFIG 0x2242 +#define MSP2_SET_MZTC_PRESET 0x2243 +#define MSP2_SET_MZTC_PALETTE 0x2244 +#define MSP2_SET_MZTC_ZOOM 0x2245 +#define MSP2_SET_MZTC_SHUTTER 0x2246 +#define MSP2_SET_MZTC_IMAGE_PARAMS 0x2247 +#define MSP2_SET_MZTC_CORRECTION 0x2248 +#define MSP2_SET_MZTC_VIGNETTING 0x2249 + +/* + * Payload layouts + * + * MSP2_MZTC_CONFIG (out) and MSP2_SET_MZTC_CONFIG (in), 11 bytes. + * The port and its baud rate are not here. They come from the Ports tab. + * u8 preset + * u8 palette_mode + * u8 auto_shutter + * u8 digital_enhancement + * u8 spatial_denoise + * u8 temporal_denoise + * u8 brightness + * u8 contrast + * u8 zoom_level + * u8 mirror_mode + * u8 ffc_interval + * + * MSP2_MZTC_STATUS (out), 7 bytes: + * u8 status + * u8 preset + * u8 connected + * u8 connection_quality + * u16 last_calibration (minutes) + * u8 error_flags + * + * MSP2_SET_MZTC_PRESET (in), 1 byte: u8 preset + * MSP2_SET_MZTC_PALETTE (in), 1 byte: u8 palette + * MSP2_SET_MZTC_ZOOM (in), 1 byte: u8 zoom_level + * MSP2_SET_MZTC_SHUTTER (in), 0 or 1 bytes. Triggers a manual shutter cycle, + * which is the camera's flat field correction. Any + * payload byte is ignored. + * MSP2_SET_MZTC_IMAGE_PARAMS (in), 3 bytes: u8 brightness, u8 contrast, + * u8 enhancement + * MSP2_SET_MZTC_CORRECTION (in), 2 bytes: u8 spatial, u8 temporal + * MSP2_SET_MZTC_VIGNETTING (in), 0 or 1 bytes. Runs one vignetting + * correction. The camera manual requires the lens to be + * pointed at a uniform surface first, so this is an + * action and never a stored setting. + */ + +#define MSP2_MZTC_CONFIG_PAYLOAD_SIZE 11 +#define MSP2_MZTC_STATUS_PAYLOAD_SIZE 7 +#define MSP2_SET_MZTC_IMAGE_PARAMS_PAYLOAD_SIZE 3 +#define MSP2_SET_MZTC_CORRECTION_PAYLOAD_SIZE 2 + +#endif // USE_MZTC diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index b1d5b8713aa..e4eb8a9b82a 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -97,6 +97,9 @@ typedef enum { #ifdef USE_OSD TASK_OSD, #endif +#ifdef USE_MZTC + TASK_MZTC_CAMERA, +#endif #ifdef USE_CMS TASK_CMS, #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index 4b23d187099..b288871cbf0 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -433,6 +433,12 @@ #define USE_HEADTRACKER_MSP #endif +// Thermal Camera Support (off 512 KB targets to preserve flash space; +// SITL has no flash constraint — SITL_BUILD is set by cmake/sitl.cmake) +#if defined(SITL_BUILD) || (MCU_FLASH_SIZE > 512) +#define USE_MZTC +#endif + #if defined(STM32F7) || defined(STM32H7) // needs bi-direction inverter, not available on F4 hardware. #define USE_TELEMETRY_SBUS2 diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 95d75fc335c..78fa5b28d76 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -30,6 +30,10 @@ set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c") +set_property(SOURCE mztc_camera_unittest.cc PROPERTY depends + "io/mztc_camera.c" "common/maths.c") +set_property(SOURCE mztc_camera_unittest.cc PROPERTY definitions USE_MZTC) + set_property(SOURCE rcdevice_unittest.cc PROPERTY definitions USE_RCDEVICE) set_property(SOURCE rcdevice_unittest.cc PROPERTY depends "common/bitarray.c" "common/crc.c" "io/rcdevice.c" "io/rcdevice_cam.c" diff --git a/src/test/unit/mztc_camera_unittest.cc b/src/test/unit/mztc_camera_unittest.cc new file mode 100644 index 00000000000..067f788d49e --- /dev/null +++ b/src/test/unit/mztc_camera_unittest.cc @@ -0,0 +1,819 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "unittest_macros.h" + +extern "C" { +#include "platform.h" +#include "common/utils.h" +#include "config/mztc_camera.h" +#include "io/mztc_camera.h" +// The driver reads the THERMAL CALIBRATE switch, so the test provides +// IS_RC_MODE_ACTIVE and needs the box id enum. +#include "fc/rc_modes.h" + +// Driver internals the receive-path tests drive directly. These are static in +// production builds and visible here through STATIC_UNIT_TESTED. +void mztcSerialReceiveCallback(uint16_t c, void *rxCallbackData); +void mztcSendConfiguration(void); + +// Test hooks defined in the driver under UNIT_TEST. +void mztcTestReset(void); +void mztcTestForceReinit(void); +void mztcTestSetStatus(uint8_t status); +void mztcTestSetLastCalibration(uint16_t minutes); +} + +/* + * These tests exercise what can be checked without a camera attached: the + * serial wire format and the configuration validator that the MSP set handler + * relies on. The wire format is pinned against a packet captured from real + * hardware. A regression in the framing math fails here, before it reaches a + * bench. + */ + +// Captured from a MassZero camera: read device model, no payload. +// begin, size, address, class, subclass, flags, checksum, end. +static const uint8_t READ_MODEL_PACKET[] = { 0xF0, 0x04, 0x36, 0x74, 0x02, 0x01, 0xAD, 0xFF }; + +// Whether the Ports tab has MZTC_CAMERA assigned to a UART. +static bool mztcPortAssigned; + +// Whether the THERMAL CALIBRATE switch is held. The driver triggers one flat +// field correction on the rising edge. +static bool mztcCalibrateBoxActive; + +class MztcFramingTest : public ::testing::Test { +protected: + uint8_t packet[MZTC_MAX_PACKET_LEN]; +}; + +TEST_F(MztcFramingTest, ZeroPayloadCommandMatchesHardwareCapture) +{ + const uint8_t len = mztcBuildPacket(packet, 0x74, 0x02, 0x01, NULL, 0); + + ASSERT_EQ(sizeof(READ_MODEL_PACKET), len); + EXPECT_EQ(0, memcmp(READ_MODEL_PACKET, packet, len)); +} + +TEST_F(MztcFramingTest, TotalLengthIsPayloadPlusOverhead) +{ + const uint8_t payload[MZTC_MAX_DATA_LEN] = { 0 }; + + for (uint8_t dataLen = 0; dataLen <= MZTC_MAX_DATA_LEN; dataLen++) { + const uint8_t len = mztcBuildPacket(packet, 0x78, 0x02, 0x00, payload, dataLen); + EXPECT_EQ(MZTC_PACKET_OVERHEAD + dataLen, len) << "data_len " << (int)dataLen; + } +} + +TEST_F(MztcFramingTest, SizeFieldIsPayloadPlusFour) +{ + const uint8_t payload[3] = { 0x11, 0x22, 0x33 }; + const uint8_t len = mztcBuildPacket(packet, 0x78, 0x02, 0x00, payload, sizeof(payload)); + + ASSERT_NE(0, len); + EXPECT_EQ(sizeof(payload) + MZTC_SIZE_FIELD_OFFSET, packet[1]); + // The size field describes address through checksum. The wire length is the + // size field plus the begin and end markers plus the size byte itself. + EXPECT_EQ(packet[1] + 4, len); +} + +TEST_F(MztcFramingTest, ChecksumAndTerminatorAreAlwaysPresent) +{ + const uint8_t payload[5] = { 0xF0, 0xFF, 0x00, 0xFF, 0xF0 }; + const uint8_t len = mztcBuildPacket(packet, 0x7C, 0x04, 0x00, payload, sizeof(payload)); + + ASSERT_NE(0, len); + EXPECT_EQ(MZTC_PACKET_END, packet[len - 1]); + + uint8_t expected = 0; + for (uint8_t i = 2; i < (uint8_t)(len - 2); i++) { + expected += packet[i]; + } + EXPECT_EQ(expected, packet[len - 2]); +} + +TEST_F(MztcFramingTest, PayloadBytesAreCopiedVerbatim) +{ + const uint8_t payload[4] = { 0xDE, 0xAD, 0xBE, 0xEF }; + const uint8_t len = mztcBuildPacket(packet, 0x78, 0x10, 0x00, payload, sizeof(payload)); + + ASSERT_NE(0, len); + EXPECT_EQ(0, memcmp(payload, &packet[6], sizeof(payload))); +} + +TEST_F(MztcFramingTest, OversizedPayloadIsRejected) +{ + const uint8_t payload[MZTC_MAX_DATA_LEN + 1] = { 0 }; + + EXPECT_EQ(0, mztcBuildPacket(packet, 0x78, 0x02, 0x00, payload, MZTC_MAX_DATA_LEN + 1)); +} + +TEST_F(MztcFramingTest, NullPayloadWithNonZeroLengthIsRejected) +{ + EXPECT_EQ(0, mztcBuildPacket(packet, 0x78, 0x02, 0x00, NULL, 4)); +} + +TEST_F(MztcFramingTest, BuiltPacketsValidate) +{ + const uint8_t payload[MZTC_MAX_DATA_LEN] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14 }; + + for (uint8_t dataLen = 0; dataLen <= MZTC_MAX_DATA_LEN; dataLen++) { + const uint8_t len = mztcBuildPacket(packet, 0x74, 0x02, 0x03, payload, dataLen); + ASSERT_NE(0, len); + EXPECT_TRUE(mztcPacketIsValid(packet, len)) << "data_len " << (int)dataLen; + } +} + +TEST_F(MztcFramingTest, HardwareCaptureValidates) +{ + EXPECT_TRUE(mztcPacketIsValid(READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET))); +} + +TEST_F(MztcFramingTest, CorruptChecksumIsRejected) +{ + memcpy(packet, READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET)); + packet[6] ^= 0xFF; + + EXPECT_FALSE(mztcPacketIsValid(packet, sizeof(READ_MODEL_PACKET))); +} + +TEST_F(MztcFramingTest, WrongDeviceAddressIsRejected) +{ + memcpy(packet, READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET)); + packet[2] = 0x37; + packet[6] += 1; // keep the checksum consistent so only the address is wrong + + EXPECT_FALSE(mztcPacketIsValid(packet, sizeof(READ_MODEL_PACKET))); +} + +TEST_F(MztcFramingTest, DeclaredLengthMustMatchReceivedLength) +{ + memcpy(packet, READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET)); + packet[1] = 0x05; + + EXPECT_FALSE(mztcPacketIsValid(packet, sizeof(READ_MODEL_PACKET))); +} + +TEST_F(MztcFramingTest, MissingMarkersAreRejected) +{ + memcpy(packet, READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET)); + packet[0] = 0x00; + EXPECT_FALSE(mztcPacketIsValid(packet, sizeof(READ_MODEL_PACKET))); + + memcpy(packet, READ_MODEL_PACKET, sizeof(READ_MODEL_PACKET)); + packet[sizeof(READ_MODEL_PACKET) - 1] = 0x00; + EXPECT_FALSE(mztcPacketIsValid(packet, sizeof(READ_MODEL_PACKET))); +} + +TEST_F(MztcFramingTest, RuntPacketIsRejected) +{ + EXPECT_FALSE(mztcPacketIsValid(READ_MODEL_PACKET, MZTC_MIN_PACKET_LEN - 1)); + EXPECT_FALSE(mztcPacketIsValid(NULL, MZTC_MIN_PACKET_LEN)); +} + +class MztcConfigValidationTest : public ::testing::Test { +protected: + void SetUp() override + { + memset(&config, 0, sizeof(config)); + config.preset = MZTC_PRESET_CUSTOM; + config.palette_mode = MZTC_PALETTE_WHITE_HOT; + config.auto_shutter = MZTC_SHUTTER_TIME_AND_TEMP; + config.digital_enhancement = 50; + config.spatial_denoise = 50; + config.temporal_denoise = 50; + config.brightness = 50; + config.contrast = 50; + config.zoom_level = MZTC_ZOOM_1X; + config.mirror_mode = MZTC_MIRROR_NONE; + config.ffc_interval = 5; + } + + mztcConfig_t config; +}; + +TEST_F(MztcConfigValidationTest, DefaultsAreAccepted) +{ + EXPECT_TRUE(mztcConfigIsValid(&config)); +} + +// Every preset value must round-trip the validator, since selecting one writes +// these fields directly into the running configuration. +TEST_F(MztcConfigValidationTest, EveryPresetProducesAValidConfig) +{ + for (int p = MZTC_PRESET_CUSTOM; p <= MZTC_PRESET_MARITIME; p++) { + mztcTestReset(); + mztcPortAssigned = true; + // Start from the valid baseline the parameter group resets to. Custom + // writes nothing, so it cannot repair a configuration that was already + // out of range. + *mztcConfigMutable() = config; + + ASSERT_TRUE(mztcSetPreset((mztcPreset_e)p)) << "preset " << p; + EXPECT_TRUE(mztcConfigIsValid(mztcConfig())) << "preset " << p; + EXPECT_EQ(p, mztcConfig()->preset) << "preset " << p; + } +} + +// Custom deliberately writes nothing, so it does not rescue an invalid +// configuration. This pins that behaviour rather than leaving it implied. +TEST_F(MztcConfigValidationTest, CustomDoesNotRepairAnInvalidConfig) +{ + mztcTestReset(); + mztcPortAssigned = true; + memset(mztcConfigMutable(), 0, sizeof(mztcConfig_t)); + + ASSERT_TRUE(mztcSetPreset(MZTC_PRESET_CUSTOM)); + EXPECT_FALSE(mztcConfigIsValid(mztcConfig())); +} + +// CUSTOM is the escape hatch. It must not overwrite a hand-tuned value. +TEST_F(MztcConfigValidationTest, CustomPresetWritesNothing) +{ + mztcTestReset(); + mztcPortAssigned = true; + *mztcConfigMutable() = config; + mztcConfigMutable()->brightness = 17; + mztcConfigMutable()->palette_mode = MZTC_PALETTE_SEPIA; + + ASSERT_TRUE(mztcSetPreset(MZTC_PRESET_CUSTOM)); + + EXPECT_EQ(17, mztcConfig()->brightness); + EXPECT_EQ(MZTC_PALETTE_SEPIA, mztcConfig()->palette_mode); +} + +// A named preset must actually change the owned settings, and must leave the +// two it does not own alone. +TEST_F(MztcConfigValidationTest, PresetWritesOwnedFieldsAndSparesTheRest) +{ + mztcTestReset(); + mztcPortAssigned = true; + *mztcConfigMutable() = config; + mztcConfigMutable()->zoom_level = MZTC_ZOOM_4X; + mztcConfigMutable()->mirror_mode = MZTC_MIRROR_VERTICAL; + + ASSERT_TRUE(mztcSetPreset(MZTC_PRESET_FIRE)); + + EXPECT_EQ(MZTC_PALETTE_IRON_RED_1, mztcConfig()->palette_mode); + EXPECT_EQ(75, mztcConfig()->contrast); + EXPECT_EQ(25, mztcConfig()->digital_enhancement); + EXPECT_EQ(10, mztcConfig()->ffc_interval); + + // Zoom belongs to the pilot, mirror to the airframe. + EXPECT_EQ(MZTC_ZOOM_4X, mztcConfig()->zoom_level); + EXPECT_EQ(MZTC_MIRROR_VERTICAL, mztcConfig()->mirror_mode); +} + +TEST_F(MztcConfigValidationTest, OutOfRangePresetIsRejected) +{ + mztcTestReset(); + mztcPortAssigned = true; + EXPECT_FALSE(mztcSetPreset((mztcPreset_e)(MZTC_PRESET_MARITIME + 1))); +} + +TEST_F(MztcConfigValidationTest, EnumsAreBounded) +{ + config.preset = MZTC_PRESET_MARITIME + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.preset = MZTC_PRESET_MARITIME; + + config.palette_mode = MZTC_PALETTE_RED_HOT + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.palette_mode = MZTC_PALETTE_RED_HOT; + + config.zoom_level = MZTC_ZOOM_8X + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.zoom_level = MZTC_ZOOM_8X; + + config.mirror_mode = MZTC_MIRROR_CENTRAL + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.mirror_mode = MZTC_MIRROR_CENTRAL; + + config.auto_shutter = MZTC_SHUTTER_TIME_AND_TEMP + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.auto_shutter = MZTC_SHUTTER_TIME_AND_TEMP; + + EXPECT_TRUE(mztcConfigIsValid(&config)); +} + +TEST_F(MztcConfigValidationTest, PercentagesAreBounded) +{ + config.brightness = MZTC_MAX_PERCENT + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.brightness = MZTC_MAX_PERCENT; + + config.contrast = MZTC_MAX_PERCENT + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.contrast = MZTC_MAX_PERCENT; + + config.spatial_denoise = MZTC_MAX_PERCENT + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + config.spatial_denoise = MZTC_MAX_PERCENT; + + EXPECT_TRUE(mztcConfigIsValid(&config)); +} + +TEST_F(MztcConfigValidationTest, FfcIntervalIsBounded) +{ + config.ffc_interval = 0; + EXPECT_FALSE(mztcConfigIsValid(&config)); + + config.ffc_interval = MZTC_MAX_FFC_INTERVAL + 1; + EXPECT_FALSE(mztcConfigIsValid(&config)); + + config.ffc_interval = MZTC_MIN_FFC_INTERVAL; + EXPECT_TRUE(mztcConfigIsValid(&config)); + + config.ffc_interval = MZTC_MAX_FFC_INTERVAL; + EXPECT_TRUE(mztcConfigIsValid(&config)); +} + +// The camera accepts 0x01 to 0x03 for the shutter mode and answers 0x00 with a +// threshold error, so every setting value has to land inside that window once +// the wire offset is applied. +TEST_F(MztcConfigValidationTest, ShutterModeMapsIntoTheCameraWireRange) +{ + for (uint8_t mode = MZTC_SHUTTER_TEMP_ONLY; mode <= MZTC_SHUTTER_TIME_AND_TEMP; mode++) { + const uint8_t wire = (uint8_t)(mode + MZTC_SHUTTER_WIRE_OFFSET); + EXPECT_GE(wire, 0x01) << "mode " << (int)mode; + EXPECT_LE(wire, 0x03) << "mode " << (int)mode; + } +} + +TEST_F(MztcConfigValidationTest, NullConfigIsRejected) +{ + EXPECT_FALSE(mztcConfigIsValid(NULL)); +} + +/* + * Receive path, response dispatch and transmit path. + * + * These drive the driver through its serial callback rather than calling the + * decoders directly, so the framing, the dispatch and the connection state + * machine are all exercised on the path the camera actually uses. + */ + +// Bytes the driver has handed to the serial port since the last reset. +static std::vector txBytes; +static timeMs_t fakeNow; + + +class MztcLinkTest : public ::testing::Test { +protected: + void SetUp() override + { + txBytes.clear(); + fakeNow = 1000; + mztcPortAssigned = true; + mztcCalibrateBoxActive = false; + + mztcConfig_t *cfg = mztcConfigMutable(); + memset(cfg, 0, sizeof(*cfg)); + cfg->preset = MZTC_PRESET_CUSTOM; + cfg->palette_mode = MZTC_PALETTE_WHITE_HOT; + cfg->auto_shutter = MZTC_SHUTTER_TIME_AND_TEMP; + cfg->digital_enhancement = 50; + cfg->spatial_denoise = 50; + cfg->temporal_denoise = 50; + cfg->brightness = 50; + cfg->contrast = 50; + cfg->zoom_level = MZTC_ZOOM_1X; + cfg->mirror_mode = MZTC_MIRROR_NONE; + cfg->ffc_interval = 5; + + mztcTestReset(); + } + + // Push a whole packet through the receive callback one byte at a time. + void feed(const uint8_t *bytes, size_t len) + { + for (size_t i = 0; i < len; i++) { + mztcSerialReceiveCallback(bytes[i], NULL); + } + } + + void feedReply(uint8_t classCmd, uint8_t subCmd, uint8_t flags, + const uint8_t *payload = NULL, uint8_t payloadLen = 0) + { + uint8_t packet[MZTC_MAX_PACKET_LEN]; + const uint8_t len = mztcBuildPacket(packet, classCmd, subCmd, flags, payload, payloadLen); + ASSERT_NE(0, len); + feed(packet, len); + } + + // Find a transmitted packet with the given class and subclass. + const uint8_t *findTx(uint8_t classCmd, uint8_t subCmd, uint8_t *lenOut) const + { + size_t i = 0; + while (i + MZTC_MIN_PACKET_LEN <= txBytes.size()) { + const uint8_t len = (uint8_t)(txBytes[i + 1] + 4); + if (i + len > txBytes.size()) { + return NULL; + } + if (txBytes[i + 3] == classCmd && txBytes[i + 4] == subCmd) { + if (lenOut) { + *lenOut = len; + } + return &txBytes[i]; + } + i += len; + } + return NULL; + } + + // How many transmitted packets carry the given class and subclass. Used to + // prove an action fires once rather than on every task tick. + size_t countTx(uint8_t classCmd, uint8_t subCmd) const + { + size_t i = 0, found = 0; + while (i + MZTC_MIN_PACKET_LEN <= txBytes.size()) { + const uint8_t len = (uint8_t)(txBytes[i + 1] + 4); + if (i + len > txBytes.size()) { + break; + } + if (txBytes[i + 3] == classCmd && txBytes[i + 4] == subCmd) { + found++; + } + i += len; + } + return found; + } +}; + +TEST_F(MztcLinkTest, ValidReplyIsAccepted) +{ + const uint8_t model[3] = { 'M', 'Z', '1' }; + feedReply(0x74, 0x02, 0x03, model, sizeof(model)); + + uint8_t idLen = 0; + const uint8_t *id = mztcGetDeviceId(&idLen); + ASSERT_NE(nullptr, id); + EXPECT_EQ(sizeof(model), idLen); + EXPECT_EQ(0, memcmp(model, id, idLen)); +} + +// The parser is length driven precisely so a payload byte that happens to equal +// a framing marker cannot split or truncate the packet. +TEST_F(MztcLinkTest, PayloadContainingFramingMarkersIsParsed) +{ + const uint8_t payload[4] = { 0xF0, 0xFF, 0xFF, 0xF0 }; + feedReply(0x74, 0x02, 0x03, payload, sizeof(payload)); + + uint8_t idLen = 0; + const uint8_t *id = mztcGetDeviceId(&idLen); + ASSERT_NE(nullptr, id); + EXPECT_EQ(sizeof(payload), idLen); + EXPECT_EQ(0, memcmp(payload, id, idLen)); +} + +TEST_F(MztcLinkTest, GarbageBeforeAFrameIsSkipped) +{ + const uint8_t noise[4] = { 0x11, 0x22, 0x33, 0x44 }; + feed(noise, sizeof(noise)); + + const uint8_t model[1] = { 0x5A }; + feedReply(0x74, 0x02, 0x03, model, sizeof(model)); + + uint8_t idLen = 0; + ASSERT_NE(nullptr, mztcGetDeviceId(&idLen)); + EXPECT_EQ(1, idLen); +} + +TEST_F(MztcLinkTest, BadChecksumIsRejected) +{ + uint8_t packet[MZTC_MAX_PACKET_LEN]; + const uint8_t model[1] = { 0x5A }; + const uint8_t len = mztcBuildPacket(packet, 0x74, 0x02, 0x03, model, sizeof(model)); + packet[len - 2] ^= 0xFF; + feed(packet, len); + + uint8_t idLen = 0; + EXPECT_EQ(nullptr, mztcGetDeviceId(&idLen)); + EXPECT_FALSE(mztcGetStatus()->connected); +} + +// A size byte outside the legal range must not wedge the parser. The next +// well-formed packet has to be decoded. +TEST_F(MztcLinkTest, BogusLengthResynchronises) +{ + const uint8_t bogus[2] = { MZTC_PACKET_BEGIN, 0xFE }; + feed(bogus, sizeof(bogus)); + + const uint8_t model[1] = { 0x5A }; + feedReply(0x74, 0x02, 0x03, model, sizeof(model)); + + uint8_t idLen = 0; + ASSERT_NE(nullptr, mztcGetDeviceId(&idLen)); + EXPECT_EQ(1, idLen); +} + +TEST_F(MztcLinkTest, TruncatedPacketIsNotDecoded) +{ + uint8_t packet[MZTC_MAX_PACKET_LEN]; + const uint8_t model[2] = { 0x5A, 0x5B }; + const uint8_t len = mztcBuildPacket(packet, 0x74, 0x02, 0x03, model, sizeof(model)); + feed(packet, len - 1); + + uint8_t idLen = 0; + EXPECT_EQ(nullptr, mztcGetDeviceId(&idLen)); +} + +// The camera answers the 0x7C/0x14 initialization request on class 0x7D +// subclass 0x06. Decoding it on the request address instead never matches. +TEST_F(MztcLinkTest, InitStatusReplyUsesItsOwnAddress) +{ + mztcTestSetStatus(MZTC_STATUS_INITIALIZING); + + const uint8_t outputStage[1] = { 0x01 }; + feedReply(0x7D, 0x06, 0x03, outputStage, sizeof(outputStage)); + EXPECT_EQ(MZTC_STATUS_READY, mztcGetStatus()->status); + + const uint8_t logoStage[1] = { 0x00 }; + feedReply(0x7D, 0x06, 0x03, logoStage, sizeof(logoStage)); + EXPECT_EQ(MZTC_STATUS_INITIALIZING, mztcGetStatus()->status); +} + +TEST_F(MztcLinkTest, ShutterReplyRestartsTheCalibrationClock) +{ + mztcTestSetStatus(MZTC_STATUS_CALIBRATING); + mztcTestSetLastCalibration(42); + + feedReply(0x7C, 0x02, 0x03); + + EXPECT_EQ(0, mztcGetStatus()->last_calibration); + EXPECT_EQ(MZTC_STATUS_READY, mztcGetStatus()->status); +} + +TEST_F(MztcLinkTest, ErrorReplyRaisesTheCommunicationFlag) +{ + const uint8_t thresholdExceeded[1] = { 0x01 }; + feedReply(0x78, 0x02, 0x04, thresholdExceeded, sizeof(thresholdExceeded)); + + EXPECT_TRUE((mztcGetStatus()->error_flags & MZTC_ERROR_COMMUNICATION) != 0); +} + +TEST_F(MztcLinkTest, SuccessReplyClearsTheCommunicationFlag) +{ + const uint8_t thresholdExceeded[1] = { 0x01 }; + feedReply(0x78, 0x02, 0x04, thresholdExceeded, sizeof(thresholdExceeded)); + ASSERT_TRUE((mztcGetStatus()->error_flags & MZTC_ERROR_COMMUNICATION) != 0); + + feedReply(0x78, 0x02, 0x03); + EXPECT_FALSE((mztcGetStatus()->error_flags & MZTC_ERROR_COMMUNICATION) != 0); +} + +// Opening the port is not enough. The camera has to answer before the link +// counts as up. +TEST_F(MztcLinkTest, AnAnswerPromotesTheLinkToConnected) +{ + EXPECT_FALSE(mztcGetStatus()->connected); + + feedReply(0x74, 0x02, 0x03); + + EXPECT_TRUE(mztcGetStatus()->connected); +} + +// Assigning the function in the Ports tab is what enables the camera. There is +// no separate enable setting to fall out of step with it. +// "set mztc_preset = SEARCH" writes the byte and nothing else. The task has to +// notice and apply it, otherwise the setting records a label that does not +// match the values in effect. That is the defect the old mode enum had. +// The calibrate switch fires once per flip. Holding it must not stream shutter +// commands at the camera on every task tick. +TEST_F(MztcLinkTest, TheCalibrateSwitchFiresOnceOnTheRisingEdge) +{ + mztcUpdate(0); + feedReply(0x74, 0x02, 0x03); + mztcUpdate(0); + txBytes.clear(); + + mztcCalibrateBoxActive = true; + mztcUpdate(0); + const size_t afterFirst = countTx(0x7C, 0x02); + EXPECT_EQ(1u, afterFirst); + + // Still held. No further corrections. + mztcUpdate(0); + mztcUpdate(0); + EXPECT_EQ(afterFirst, countTx(0x7C, 0x02)); + + // Released and flipped again. One more. + mztcCalibrateBoxActive = false; + mztcUpdate(0); + mztcCalibrateBoxActive = true; + mztcUpdate(0); + EXPECT_EQ(afterFirst + 1, countTx(0x7C, 0x02)); +} + +TEST_F(MztcLinkTest, APresetWrittenAsASettingIsAppliedByTheTask) +{ + mztcUpdate(0); + feedReply(0x74, 0x02, 0x03); + ASSERT_TRUE(mztcGetStatus()->connected); + mztcUpdate(0); + + // Write the field directly, the way the settings framework does. + mztcConfigMutable()->preset = MZTC_PRESET_FIRE; + mztcUpdate(0); + + EXPECT_EQ(MZTC_PALETTE_IRON_RED_1, mztcConfig()->palette_mode); + EXPECT_EQ(75, mztcConfig()->contrast); + EXPECT_EQ(25, mztcConfig()->digital_enhancement); + EXPECT_EQ(10, mztcConfig()->ffc_interval); +} + +// Boot must not reapply. The saved values already reflect the saved preset, so +// reapplying would discard hand tuning done after the preset was chosen. +TEST_F(MztcLinkTest, BootDoesNotReapplyTheStoredPreset) +{ + mztcConfigMutable()->preset = MZTC_PRESET_FIRE; + mztcConfigMutable()->contrast = 33; // hand tuned after picking FIRE + + // Run the real boot path. mztcInit() returns early while the driver is + // already initialised, so without this the test would pass regardless. + mztcTestForceReinit(); + mztcInit(); + + mztcUpdate(0); + feedReply(0x74, 0x02, 0x03); + mztcUpdate(0); + mztcUpdate(0); + + EXPECT_EQ(33, mztcConfig()->contrast) << "boot reapplied and lost the tuning"; + EXPECT_EQ(MZTC_PRESET_FIRE, mztcConfig()->preset); +} + +TEST_F(MztcLinkTest, ThePortAssignmentIsWhatEnablesTheCamera) +{ + mztcPortAssigned = true; + EXPECT_TRUE(mztcIsEnabled()); + + mztcPortAssigned = false; + EXPECT_FALSE(mztcIsEnabled()); +} + +/* + * Transmit path + */ + +TEST_F(MztcLinkTest, ConfigurationBurstUsesTheCameraShutterWireValues) +{ + for (uint8_t mode = MZTC_SHUTTER_TEMP_ONLY; mode <= MZTC_SHUTTER_TIME_AND_TEMP; mode++) { + txBytes.clear(); + mztcConfigMutable()->auto_shutter = mode; + mztcSendConfiguration(); + + uint8_t len = 0; + const uint8_t *packet = findTx(0x7C, 0x04, &len); + ASSERT_NE(nullptr, packet) << "no auto shutter command for mode " << (int)mode; + ASSERT_EQ(MZTC_PACKET_OVERHEAD + 1, len); + + // The manual defines 0x01 temperature only, 0x02 time only and 0x03 + // time and temperature. It answers 0x00 with a threshold error. + EXPECT_EQ(mode + 1, packet[6]) << "mode " << (int)mode; + EXPECT_GE(packet[6], 0x01); + EXPECT_LE(packet[6], 0x03); + } +} + +// The camera owns the shutter schedule. The interval has to reach it. +TEST_F(MztcLinkTest, ConfigurationBurstSendsTheShutterInterval) +{ + mztcConfigMutable()->ffc_interval = 7; + mztcSendConfiguration(); + + uint8_t len = 0; + const uint8_t *packet = findTx(0x7C, 0x05, &len); + ASSERT_NE(nullptr, packet); + ASSERT_EQ(MZTC_PACKET_OVERHEAD + 2, len); + EXPECT_EQ(0, packet[6]); + EXPECT_EQ(7, packet[7]); +} + +TEST_F(MztcLinkTest, ConfigurationBurstSendsEveryImageParameter) +{ + mztcConfigMutable()->brightness = 11; + mztcConfigMutable()->contrast = 22; + mztcConfigMutable()->digital_enhancement = 33; + mztcConfigMutable()->spatial_denoise = 44; + mztcConfigMutable()->temporal_denoise = 55; + mztcConfigMutable()->palette_mode = MZTC_PALETTE_IRON_RED_1; + mztcConfigMutable()->zoom_level = MZTC_ZOOM_4X; + mztcConfigMutable()->mirror_mode = MZTC_MIRROR_VERTICAL; + mztcSendConfiguration(); + + struct { uint8_t cls; uint8_t sub; uint8_t value; const char *name; } expected[] = { + { 0x78, 0x02, 11, "brightness" }, + { 0x78, 0x03, 22, "contrast" }, + { 0x78, 0x10, 33, "digital enhancement" }, + { 0x78, 0x15, 44, "spatial denoise" }, + { 0x78, 0x16, 55, "temporal denoise" }, + { 0x78, 0x20, MZTC_PALETTE_IRON_RED_1, "palette" }, + { 0x70, 0x12, MZTC_ZOOM_4X, "zoom" }, + { 0x70, 0x11, MZTC_MIRROR_VERTICAL, "mirror" }, + }; + + for (size_t i = 0; i < ARRAYLEN(expected); i++) { + uint8_t len = 0; + const uint8_t *packet = findTx(expected[i].cls, expected[i].sub, &len); + ASSERT_NE(nullptr, packet) << expected[i].name << " was not sent"; + ASSERT_EQ(MZTC_PACKET_OVERHEAD + 1, len) << expected[i].name; + EXPECT_EQ(expected[i].value, packet[6]) << expected[i].name; + } +} + +// Everything the driver puts on the wire has to survive its own validator. +TEST_F(MztcLinkTest, EveryTransmittedPacketIsWellFormed) +{ + mztcSendConfiguration(); + ASSERT_FALSE(txBytes.empty()); + + size_t i = 0; + int packets = 0; + while (i + MZTC_MIN_PACKET_LEN <= txBytes.size()) { + const uint8_t len = (uint8_t)(txBytes[i + 1] + 4); + ASSERT_LE(i + len, txBytes.size()) << "packet " << packets << " runs past the buffer"; + EXPECT_TRUE(mztcPacketIsValid(&txBytes[i], len)) << "packet " << packets; + i += len; + packets++; + } + EXPECT_EQ(txBytes.size(), i) << "trailing bytes after the last packet"; + EXPECT_GE(packets, 9); +} + +// Everything the driver needs from the rest of the firmware. No real serial +// port is ever opened. serialWriteBufShim captures what the driver transmits so +// the transmit tests can assert on the actual wire bytes. +extern "C" { + +#include "build/debug.h" +#include "drivers/serial.h" +#include "io/serial.h" + +int32_t debug[DEBUG32_VALUE_COUNT]; + +const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, + 230400, 250000, 460800, 921600, 1000000, 1500000, 2000000, 2470000 }; + +timeMs_t millis(void) +{ + return fakeNow; +} + +// The Ports tab decides whether the camera exists. Returning a config here is +// the test equivalent of assigning MZTC_CAMERA to a UART. +static serialPortConfig_t fakePortConfig; + +bool IS_RC_MODE_ACTIVE(boxId_e boxId) +{ + return boxId == BOXMZTCCALIBRATE && mztcCalibrateBoxActive; +} + +serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) +{ + if (function != FUNCTION_MZTC_CAMERA || !mztcPortAssigned) { + return NULL; + } + fakePortConfig.identifier = SERIAL_PORT_USART2; + fakePortConfig.peripheral_baudrateIndex = 8; + return &fakePortConfig; +} + +serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, + void *, uint32_t, portMode_t, portOptions_t) +{ + return NULL; +} + +void closeSerialPort(serialPort_t *) +{ +} + +void serialWriteBufShim(void *, const uint8_t *data, int count) +{ + for (int i = 0; i < count; i++) { + txBytes.push_back(data[i]); + } +} + +} diff --git a/src/utils/test_thermal_camera.py b/src/utils/test_thermal_camera.py new file mode 100644 index 00000000000..63b80f726e0 --- /dev/null +++ b/src/utils/test_thermal_camera.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +""" +Test script to verify thermal camera communication on COM13 +""" + +import serial +import time + +# Configuration +SERIAL_PORT = 'COM13' +SERIAL_BAUD = 115200 + +def send_command(ser, class_cmd, subclass_cmd, flags=0x01, data=None): + """Send a command packet to the thermal camera""" + if data is None: + data = [] + + # Build packet + packet = bytearray() + packet.append(0xF0) # Start byte + packet.append(len(data) + 4) # Size + packet.append(0x36) # Device address + packet.append(class_cmd) + packet.append(subclass_cmd) + packet.append(flags) + packet.extend(data) + + # Calculate checksum + checksum = 0x36 + class_cmd + subclass_cmd + flags + sum(data) + packet.append(checksum & 0xFF) + packet.append(0xFF) # End byte + + # Send packet + print(f"Sending: {' '.join([f'{b:02X}' for b in packet])}") + ser.write(packet) + + # Wait for response + time.sleep(0.1) + response = ser.read(256) + if response: + print(f"Response: {' '.join([f'{b:02X}' for b in response])}") + return response + else: + print("No response") + return None + +def main(): + print("Testing MassZero Thermal Camera on COM13") + print("-" * 50) + + try: + # Open serial port + ser = serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=1.0) + print(f"Opened {SERIAL_PORT} at {SERIAL_BAUD} baud") + + # Clear any pending data + ser.read_all() + + # Test 1: Read device model + print("\n1. Reading device model...") + send_command(ser, 0x74, 0x02) + + # Test 2: Read FPGA version + print("\n2. Reading FPGA version...") + send_command(ser, 0x74, 0x03) + + # Test 3: Read software version + print("\n3. Reading software version...") + send_command(ser, 0x74, 0x05) + + # Test 4: Read initialization status + print("\n4. Reading initialization status...") + send_command(ser, 0x7C, 0x14) + + # Test 5: Try to set brightness (write command) + print("\n5. Setting brightness to 50...") + send_command(ser, 0x78, 0x02, flags=0x00, data=[50]) + + ser.close() + + except Exception as e: + print(f"Error: {e}") + +if __name__ == "__main__": + main() + diff --git a/src/utils/thermal_bridge.py b/src/utils/thermal_bridge.py new file mode 100644 index 00000000000..6f76e39c069 --- /dev/null +++ b/src/utils/thermal_bridge.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +""" +Simple TCP-Serial bridge for connecting Windows COM port to INAV SITL in WSL +""" + +import serial +import socket +import threading +import sys + +# Configuration +SERIAL_PORT = 'COM13' # Your thermal camera port +SERIAL_BAUD = 115200 +TCP_HOST = '172.18.102.124' # Connect to localhost (SITL) +TCP_PORT = 5762 + +def forward_data(source, destination, name): + """Forward data from source to destination""" + try: + while True: + if hasattr(source, 'read'): # Serial source + data = source.read(1024) + else: # Socket source + data = source.recv(1024) + if not data: + print(f"{name}: Connection closed") + break + + if data: + if hasattr(destination, 'write'): # Serial destination + destination.write(data) + else: # Socket destination + destination.send(data) + # Enhanced debugging with hex dump + hex_str = ' '.join([f'{b:02X}' for b in data[:20]]) + if len(data) > 20: + hex_str += '...' + print(f"{name}: {len(data)} bytes: {hex_str}") + except Exception as e: + print(f"{name}: Error - {e}") + +def main(): + print(f"MassZero Thermal Camera Bridge") + print(f"Serial: {SERIAL_PORT} @ {SERIAL_BAUD}") + print(f"TCP: Connecting to {TCP_HOST}:{TCP_PORT}") + + # Open serial port + try: + ser = serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=0.1) + print(f"Opened {SERIAL_PORT}") + except Exception as e: + print(f"Failed to open {SERIAL_PORT}: {e}") + sys.exit(1) + + # Connect to SITL's TCP server + try: + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.connect((TCP_HOST, TCP_PORT)) + print(f"Connected to SITL on {TCP_HOST}:{TCP_PORT}") + + # Create forwarding threads + t1 = threading.Thread(target=forward_data, + args=(ser, sock, "Serial->TCP")) + t2 = threading.Thread(target=forward_data, + args=(sock, ser, "TCP->Serial")) + + t1.daemon = True + t2.daemon = True + t1.start() + t2.start() + + # Wait for threads to finish + t1.join() + t2.join() + + except Exception as e: + print(f"Failed to connect to SITL: {e}") + finally: + ser.close() + sock.close() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/utils/thermal_bridge_debug.py b/src/utils/thermal_bridge_debug.py new file mode 100644 index 00000000000..91a71bef08c --- /dev/null +++ b/src/utils/thermal_bridge_debug.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +""" +Debug version of TCP-Serial bridge with better logging +""" + +import serial +import socket +import threading +import sys +import time + +# Configuration +SERIAL_PORT = 'COM13' # Your thermal camera port +SERIAL_BAUD = 115200 +TCP_HOST = '172.18.102.124' # Connect to localhost (SITL) +TCP_PORT = 5762 + +def forward_serial_to_tcp(ser, sock): + """Forward data from serial to TCP with detailed logging""" + print("Serial->TCP thread started") + while True: + try: + # Read with timeout + data = ser.read(1024) + if data: + # Log the actual data received + hex_str = ' '.join([f'{b:02X}' for b in data]) + print(f"[SERIAL RX] {len(data)} bytes: {hex_str}") + + # Send to TCP + sent = sock.send(data) + print(f"[TCP TX] Sent {sent} bytes") + except Exception as e: + print(f"Serial->TCP Error: {e}") + break + +def forward_tcp_to_serial(sock, ser): + """Forward data from TCP to serial with detailed logging""" + print("TCP->Serial thread started") + while True: + try: + data = sock.recv(1024) + if not data: + print("TCP connection closed") + break + + # Log the data + hex_str = ' '.join([f'{b:02X}' for b in data]) + print(f"[TCP RX] {len(data)} bytes: {hex_str}") + + # Send to serial + written = ser.write(data) + ser.flush() + print(f"[SERIAL TX] Sent {written} bytes") + + except Exception as e: + print(f"TCP->Serial Error: {e}") + break + +def main(): + print(f"MassZero Thermal Camera Bridge (DEBUG MODE)") + print(f"Serial: {SERIAL_PORT} @ {SERIAL_BAUD}") + print(f"TCP: Connecting to {TCP_HOST}:{TCP_PORT}") + print("-" * 50) + + # Open serial port + try: + ser = serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=0.1) + print(f"✓ Opened {SERIAL_PORT}") + + # Clear any pending data + ser.read_all() + print("✓ Cleared serial buffer") + + except Exception as e: + print(f"✗ Failed to open {SERIAL_PORT}: {e}") + sys.exit(1) + + # Connect to SITL's TCP server + try: + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.connect((TCP_HOST, TCP_PORT)) + print(f"✓ Connected to SITL on {TCP_HOST}:{TCP_PORT}") + + # Test: Send a read model command to see if camera responds + print("\nSending test command to camera...") + test_cmd = bytearray([0xF0, 0x04, 0x36, 0x74, 0x02, 0x01, 0xAD, 0xFF]) + ser.write(test_cmd) + ser.flush() + time.sleep(0.2) + + response = ser.read(256) + if response: + hex_str = ' '.join([f'{b:02X}' for b in response]) + print(f"Camera test response: {hex_str}") + else: + print("No response from camera to test command") + + print("\nStarting bridge threads...") + + # Create forwarding threads + t1 = threading.Thread(target=forward_serial_to_tcp, + args=(ser, sock)) + t2 = threading.Thread(target=forward_tcp_to_serial, + args=(sock, ser)) + + t1.daemon = True + t2.daemon = True + t1.start() + t2.start() + + print("Bridge is running. Press Ctrl+C to stop.\n") + + # Wait for threads + t1.join() + t2.join() + + except Exception as e: + print(f"✗ Failed to connect to SITL: {e}") + finally: + ser.close() + sock.close() + +if __name__ == "__main__": + main() +