COTPdoMapWrite: Add error checking to prevent uninitialized variable access.

[ezrec]

Describe the bug
COTPdoMapWrite does not have error checking on its read and write index operations, which lead to tests/unit/object/cia301/co_pdo_map being flaky (as it was missing the h1A00.0 SDO entry, and the
(failing) accesses to 1A00.0 in COTPdoMapWrite() were being ignored, and random stack data in 'mapn'
was used for calculations).

To Reproduce
Steps to reproduce the behavior:

1. Run unit tests on gcc-12

Expected behavior

• tests/unit/object/cia301/co_pdo_map passes reliably.

CANopen Stack (please complete the following information):

• Device: Unit tests
• OS: Linux
• Version: develop branch

Additional context

• Discovered via valgrind