From 701da6c30f0e7e263ab90656dffa7aae83c20280 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 28 Feb 2012 09:42:34 +1100 Subject: [PATCH] AP_Param: fixed v.load() on a sub-element of a AP_Vector3f this isn't actually used at the moment in APM, but we should get it right in case someone does try to load a single element of a vector --- libraries/AP_Common/AP_Param.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp index da0ed84984..363e9d784a 100644 --- a/libraries/AP_Common/AP_Param.cpp +++ b/libraries/AP_Common/AP_Param.cpp @@ -631,8 +631,19 @@ bool AP_Param::load(void) return false; } + if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) { + // only vector3f can have non-zero idx for now + return false; + } + + AP_Param *ap; + ap = this; + if (idx != 0) { + ap = (AP_Param *)((uintptr_t)ap) - (idx*sizeof(float)); + } + // found it - eeprom_read_block(this, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); + eeprom_read_block(ap, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); return true; }