mirror of https://github.com/ArduPilot/ardupilot
Improved test program for EEProm, and added sync option, still some issues to work out.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1242 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
87307484b1
commit
f8f9575c3d
|
@ -8,13 +8,20 @@
|
|||
class AP_EEPromEntry
|
||||
{
|
||||
public:
|
||||
AP_EEPromEntry(const char * name = "") : _id(0), _address(0), _name(name), _sync(false)
|
||||
{
|
||||
}
|
||||
virtual void setEntry(float val) = 0;
|
||||
virtual float getEntry() = 0;
|
||||
uint16_t getId() { return _id; }
|
||||
uint16_t getAddress() { return _address; }
|
||||
const char * getName() { return _name; }
|
||||
bool setSync(bool sync) { _sync = sync; }
|
||||
protected:
|
||||
uint16_t _id;
|
||||
uint16_t _address;
|
||||
const char * _name;
|
||||
bool _sync;
|
||||
};
|
||||
|
||||
class AP_EEPromRegistry : public AP_Vector<AP_EEPromEntry *>
|
||||
|
@ -53,12 +60,18 @@ template <class type>
|
|||
class AP_EEPromVar : public AP_EEPromEntry
|
||||
{
|
||||
public:
|
||||
AP_EEPromVar()
|
||||
AP_EEPromVar(const char * name = "") : AP_EEPromEntry(name)
|
||||
{
|
||||
eepromRegistry.add(this,_id,_address,sizeof(_data));
|
||||
}
|
||||
void set(type val) { _data = val; }
|
||||
type get() { return _data; }
|
||||
void set(type val) {
|
||||
if (_sync) save();
|
||||
_data = val;
|
||||
}
|
||||
type get() {
|
||||
if (_sync) load();
|
||||
return _data;
|
||||
}
|
||||
void save()
|
||||
{
|
||||
eeprom_write_block((const void*)&_data,(void*)(_address),sizeof(_data));
|
||||
|
@ -70,9 +83,11 @@ public:
|
|||
virtual void setEntry(float val)
|
||||
{
|
||||
_data = (type)val;
|
||||
if (_sync) save();
|
||||
}
|
||||
virtual float getEntry()
|
||||
{
|
||||
if (_sync) load();
|
||||
return (float)_data;
|
||||
}
|
||||
private:
|
||||
|
|
|
@ -5,7 +5,8 @@
|
|||
#include <AP_Common.h>
|
||||
#include <AP_EEProm.h>
|
||||
|
||||
AP_EEPromVar<int> var;
|
||||
AP_EEPromVar<int> var("TEST_VAR1");
|
||||
AP_EEPromVar<float> var2("TEST_VAR2");
|
||||
FastSerialPort0(Serial);
|
||||
|
||||
void setup()
|
||||
|
@ -17,17 +18,66 @@ void setup()
|
|||
|
||||
void loop()
|
||||
{
|
||||
Serial.print("\n\nUn-synced variable demo (default)\n");
|
||||
|
||||
var.setSync(false);
|
||||
var.set(123);
|
||||
Serial.printf_P(PSTR("initially set 123 and save: %d\n"), var.get());
|
||||
Serial.printf_P(PSTR("\nvar.setSync(false): var.set(123): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
var.save();
|
||||
var.set(456);
|
||||
Serial.printf_P(PSTR("next set to 456: %d\n"), var.get());
|
||||
Serial.printf_P(PSTR("\nvar.save(): var.set(456): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
var.load();
|
||||
Serial.printf_P(PSTR("now reload initial value: %d\n"), var.get());
|
||||
Serial.printf_P(PSTR("\nval.load(): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
uint16_t id = var.getId();
|
||||
Serial.printf_P(PSTR("now find id for variable: %d\n"), id);
|
||||
Serial.printf_P(PSTR("now find variable value by id: %f\n"), eepromRegistry(id)->getEntry());
|
||||
Serial.printf_P(PSTR("\nvar.getId(): %d\n"), id);
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry(id)->getEntry());
|
||||
delay(2000);
|
||||
|
||||
eepromRegistry(id)->setEntry(456);
|
||||
Serial.printf_P(PSTR("now set variable value by id to 456: %d\n"), var.get());
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(456): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry(id)->getName());
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry(id)->getAddress()));
|
||||
|
||||
|
||||
Serial.print("\n\nSynced variable demo\n");
|
||||
|
||||
var2.setSync(true);
|
||||
var2.set(1.23);
|
||||
Serial.printf_P(PSTR("\nvar2.setSync(false): var2.set(1.23): %f\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
var2.save();
|
||||
var2.set(4.56);
|
||||
Serial.printf_P(PSTR("\nvar2.save(): var2.set(4.56): %f\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
var2.load();
|
||||
Serial.printf_P(PSTR("\nvar2.load(): %f\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
id = var2.getId();
|
||||
Serial.printf_P(PSTR("\nvar2.getId(): %d\n"), id);
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry(id)->getEntry());
|
||||
delay(2000);
|
||||
|
||||
eepromRegistry(id)->setEntry(456);
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(456): %d\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry(id)->getName());
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry(id)->getAddress()));
|
||||
|
||||
delay(5000);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue