From 011fdf6e2b14e4bcac7d8845a2a9542f5cbed0a4 Mon Sep 17 00:00:00 2001 From: QioTek Date: Wed, 15 Mar 2023 11:35:27 +0800 Subject: [PATCH] AP_RAMTRON: added PB85RS128C and PB85RS2MC --- libraries/AP_RAMTRON/AP_RAMTRON.cpp | 17 +++++++++++++++++ libraries/AP_RAMTRON/AP_RAMTRON.h | 1 + 2 files changed, 18 insertions(+) diff --git a/libraries/AP_RAMTRON/AP_RAMTRON.cpp b/libraries/AP_RAMTRON/AP_RAMTRON.cpp index 0654aab0fb..50a24a4990 100644 --- a/libraries/AP_RAMTRON/AP_RAMTRON.cpp +++ b/libraries/AP_RAMTRON/AP_RAMTRON.cpp @@ -40,6 +40,9 @@ const AP_RAMTRON::ramtron_id AP_RAMTRON::ramtron_ids[] = { { 0x05, 0x09, 32, 2, RDID_type::Fujitsu }, // MB85RS256B { 0x24, 0x03, 16, 2, RDID_type::Fujitsu }, // MB85RS128TY { 0x25, 0x03, 32, 2, RDID_type::Fujitsu }, // MB85RS256TY + + { 0x22, 0x00, 16, 2, RDID_type::Petabytes }, // PB85RS128C + { 0x24, 0x00, 256, 3, RDID_type::Petabytes }, // PB85RS2MC }; // initialise the driver @@ -62,6 +65,11 @@ bool AP_RAMTRON::init(void) uint8_t id1; uint8_t id2; }; + struct petabytes_rdid { + uint8_t manufacturer[2]; + uint8_t id1; + uint8_t id2; + }; uint8_t rdid[sizeof(cypress_rdid)]; @@ -86,6 +94,15 @@ bool AP_RAMTRON::init(void) ramtron_ids[i].id2 == fujitsu->id2) { id = i; break; + } + } else if (ramtron_ids[i].rdid_type == RDID_type::Petabytes) { + const petabytes_rdid *petabytes = (const petabytes_rdid *)rdid; + if (petabytes->manufacturer[0] == 0x62 && + petabytes->manufacturer[1] == 0x8C && + ramtron_ids[i].id1 == petabytes->id1 && + ramtron_ids[i].id2 == petabytes->id2) { + id = i; + break; } } } diff --git a/libraries/AP_RAMTRON/AP_RAMTRON.h b/libraries/AP_RAMTRON/AP_RAMTRON.h index dea6d3703c..7174b0b0ea 100644 --- a/libraries/AP_RAMTRON/AP_RAMTRON.h +++ b/libraries/AP_RAMTRON/AP_RAMTRON.h @@ -28,6 +28,7 @@ private: enum class RDID_type :uint8_t { Cypress, Fujitsu, + Petabytes }; struct ramtron_id {