mirror of https://github.com/ArduPilot/ardupilot
HAL_FLYMAPLE: initial port to flymaple board
See libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt
This commit is contained in:
Mike McCauley
Andrew Tridgell
parent
2fa0c39f3a
commit
9bfc52d9af
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_H__
|
||||
#define __AP_HAL_FLYMAPLE_H__
|
||||
|
||||
/* Your layer exports should depend on AP_HAL.h ONLY. */
|
||||
#include <AP_HAL.h>
|
||||
|
||||
/**
|
||||
* Umbrella header for AP_HAL_FLYMAPLE module.
|
||||
* The module header exports singleton instances which must conform the
|
||||
* AP_HAL::HAL interface. It may only expose implementation details (class
|
||||
* names, headers) via the FLYMAPLE namespace.
|
||||
* The class implementing AP_HAL::HAL should be called HAL_FLYMAPLE and exist
|
||||
* in the global namespace. There should be a single const instance of the
|
||||
* HAL_FLYMAPLE class called AP_HAL_FLYMAPLE, instantiated in the HAL_FLYMAPLE_Class.cpp
|
||||
* and exported as `extern const HAL_FLYMAPLE AP_HAL_FLYMAPLE;` in HAL_FLYMAPLE_Class.h
|
||||
*
|
||||
* All declaration and compilation should be guarded by CONFIG_HAL_BOARD macros.
|
||||
* In this case, we're using CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE.
|
||||
* When creating a new HAL, declare a new HAL_BOARD_ in AP_HAL/AP_HAL_Boards.h
|
||||
*
|
||||
* The module should also export an appropriate AP_HAL_MAIN() macro iff the
|
||||
* appropriate CONFIG_HAL_BOARD value is set.
|
||||
* The AP_HAL_MAIN macro expands to a main function (either an `int main (void)`
|
||||
* or `int main (int argc, const char * argv[]), depending on platform) of an
|
||||
* ArduPilot application, whose entry points are the c++ functions
|
||||
* `void setup()` and `void loop()`, ala Arduino.
|
||||
*/
|
||||
|
||||
#include "HAL_FLYMAPLE_Class.h"
|
||||
#include "AP_HAL_FLYMAPLE_Main.h"
|
||||
|
||||
#endif //__AP_HAL_FLYMAPLE_H__
|
||||
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_MAIN_H__
|
||||
#define __AP_HAL_FLYMAPLE_MAIN_H__
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
#define AP_HAL_MAIN() extern "C" {\
|
||||
int main (void) {\
|
||||
hal.init(0, NULL); \
|
||||
setup();\
|
||||
hal.scheduler->system_initialized(); \
|
||||
for(;;) loop();\
|
||||
return 0;\
|
||||
}\
|
||||
}
|
||||
#endif // HAL_BOARD_FLYMAPLE
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_MAIN_H__
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_NAMESPACE_H__
|
||||
#define __AP_HAL_FLYMAPLE_NAMESPACE_H__
|
||||
|
||||
/* While not strictly required, names inside the FLYMAPLE namespace are prefixed
|
||||
* AP_HAL_FLYMAPLE_NS for clarity. (Some of our users aren't familiar with all of the
|
||||
* C++ namespace rules.)
|
||||
*/
|
||||
|
||||
namespace AP_HAL_FLYMAPLE_NS {
|
||||
class FLYMAPLEUARTDriver;
|
||||
class FLYMAPLEI2CDriver;
|
||||
class FLYMAPLESPIDeviceManager;
|
||||
class FLYMAPLESPIDeviceDriver;
|
||||
class FLYMAPLEAnalogSource;
|
||||
class FLYMAPLEAnalogIn;
|
||||
class FLYMAPLEStorage;
|
||||
class FLYMAPLEConsoleDriver;
|
||||
class FLYMAPLEGPIO;
|
||||
class FLYMAPLEDigitalSource;
|
||||
class FLYMAPLERCInput;
|
||||
class FLYMAPLERCOutput;
|
||||
class FLYMAPLESemaphore;
|
||||
class FLYMAPLEScheduler;
|
||||
class FLYMAPLEUtil;
|
||||
}
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_NAMESPACE_H__
|
||||
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_PRIVATE_H__
|
||||
#define __AP_HAL_FLYMAPLE_PRIVATE_H__
|
||||
|
||||
/* Umbrella header for all private headers of the AP_HAL_FLYMAPLE module.
|
||||
* Only import this header from inside AP_HAL_FLYMAPLE
|
||||
*/
|
||||
|
||||
#include "UARTDriver.h"
|
||||
#include "I2CDriver.h"
|
||||
#include "SPIDriver.h"
|
||||
#include "AnalogIn.h"
|
||||
#include "Storage.h"
|
||||
#include "Console.h"
|
||||
#include "GPIO.h"
|
||||
#include "RCInput.h"
|
||||
#include "RCOutput.h"
|
||||
#include "Semaphores.h"
|
||||
#include "Scheduler.h"
|
||||
#include "Util.h"
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_PRIVATE_H__
|
||||
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "FlymapleWirish.h"
|
||||
#include <AP_HAL.h>
|
||||
#include "AnalogIn.h"
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/* CHANNEL_READ_REPEAT: how many reads on a channel before using the value.
|
||||
* This seems to be determined empirically */
|
||||
#define CHANNEL_READ_REPEAT 2
|
||||
|
||||
/* Static variable instances */
|
||||
FLYMAPLEAnalogSource* FLYMAPLEAnalogIn::_channels[FLYMAPLE_INPUT_MAX_CHANNELS] = {NULL};
|
||||
int16_t FLYMAPLEAnalogIn::_num_channels = 0;
|
||||
int16_t FLYMAPLEAnalogIn::_active_channel = 0;
|
||||
uint16_t FLYMAPLEAnalogIn::_channel_repeat_count = 0;
|
||||
|
||||
FLYMAPLEAnalogIn::FLYMAPLEAnalogIn() :
|
||||
_vcc(FLYMAPLEAnalogSource(ANALOG_INPUT_BOARD_VCC))
|
||||
{}
|
||||
|
||||
void FLYMAPLEAnalogIn::init(void* machtnichts) {
|
||||
/* Register FLYMAPLEAnalogIn::_timer_event with the scheduler. */
|
||||
hal.scheduler->register_timer_process(_timer_event);
|
||||
/* Register each private channel with FLYMAPLEAnalogIn. */
|
||||
_register_channel(&_vcc);
|
||||
}
|
||||
|
||||
FLYMAPLEAnalogSource* FLYMAPLEAnalogIn::_create_channel(int16_t chnum) {
|
||||
FLYMAPLEAnalogSource *ch = new FLYMAPLEAnalogSource(chnum);
|
||||
_register_channel(ch);
|
||||
return ch;
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogIn::_register_channel(FLYMAPLEAnalogSource* ch) {
|
||||
if (_num_channels >= FLYMAPLE_INPUT_MAX_CHANNELS) {
|
||||
for(;;) {
|
||||
hal.console->print_P(PSTR(
|
||||
"Error: AP_HAL_FLYMAPLE::FLYMAPLEAnalogIn out of channels\r\n"));
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
}
|
||||
_channels[_num_channels] = ch;
|
||||
/* Need to lock to increment _num_channels as it is used
|
||||
* by the interrupt to access _channels */
|
||||
noInterrupts();
|
||||
_num_channels++;
|
||||
interrupts();
|
||||
// Start conversions:
|
||||
adc_reg_map *regs = ADC1->regs;
|
||||
regs->CR2 |= ADC_CR2_SWSTART;
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogIn::_timer_event(uint32_t t)
|
||||
{
|
||||
adc_reg_map *regs = ADC1->regs;
|
||||
if (_channels[_active_channel]->_pin == ANALOG_INPUT_NONE) {
|
||||
_channels[_active_channel]->new_sample(0);
|
||||
goto next_channel;
|
||||
}
|
||||
|
||||
if (!(regs->SR & ADC_SR_EOC))
|
||||
{
|
||||
/* ADC Conversion is still running - this should not happen, as we
|
||||
* are called at 1khz. */
|
||||
return;
|
||||
}
|
||||
|
||||
if (_num_channels == 0)
|
||||
{
|
||||
/* No channels are registered - nothing to be done. */
|
||||
return;
|
||||
}
|
||||
|
||||
_channel_repeat_count++;
|
||||
if (_channel_repeat_count < CHANNEL_READ_REPEAT ||
|
||||
!_channels[_active_channel]->reading_settled())
|
||||
{
|
||||
/* Start a new conversion, throw away the current conversion */
|
||||
regs->CR2 |= ADC_CR2_SWSTART;
|
||||
return;
|
||||
}
|
||||
|
||||
_channel_repeat_count = 0;
|
||||
{
|
||||
// Need this block because of declared variabled and goto
|
||||
uint16_t sample = (uint16)(regs->DR & ADC_DR_DATA);
|
||||
/* Give the active channel a new sample */
|
||||
_channels[_active_channel]->new_sample( sample );
|
||||
}
|
||||
next_channel:
|
||||
/* stop the previous channel, if a stop pin is defined */
|
||||
_channels[_active_channel]->stop_read();
|
||||
/* Move to the next channel */
|
||||
_active_channel = (_active_channel + 1) % _num_channels;
|
||||
/* Setup the next channel's conversion */
|
||||
_channels[_active_channel]->setup_read();
|
||||
/* Start conversion */
|
||||
regs->CR2 |= ADC_CR2_SWSTART;
|
||||
}
|
||||
|
||||
|
||||
AP_HAL::AnalogSource* FLYMAPLEAnalogIn::channel(int16_t ch)
|
||||
{
|
||||
if (ch == ANALOG_INPUT_BOARD_VCC) {
|
||||
return &_vcc;
|
||||
} else {
|
||||
return _create_channel(ch);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_ANALOGIN_H__
|
||||
#define __AP_HAL_FLYMAPLE_ANALOGIN_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
#define FLYMAPLE_INPUT_MAX_CHANNELS 12
|
||||
|
||||
// This is the pin number of the pin connected to VCC
|
||||
// this is not built in to the flymaple board, so you must connect
|
||||
// this pin to the board 3.3V VCC
|
||||
#define FLYMAPLE_VCC_ANALOG_IN_PIN 15
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEAnalogSource : public AP_HAL::AnalogSource {
|
||||
public:
|
||||
friend class AP_HAL_FLYMAPLE_NS::FLYMAPLEAnalogIn;
|
||||
|
||||
FLYMAPLEAnalogSource(uint8_t pin);
|
||||
float read_average();
|
||||
float read_latest();
|
||||
void set_pin(uint8_t p);
|
||||
void set_stop_pin(uint8_t p);
|
||||
void set_settle_time(uint16_t settle_time_ms);
|
||||
float voltage_average();
|
||||
float voltage_latest();
|
||||
float voltage_average_ratiometric();
|
||||
|
||||
/* implementation specific interface: */
|
||||
|
||||
/* new_sample(): called with value of ADC measurments, from interrput */
|
||||
void new_sample(uint16_t);
|
||||
|
||||
/* setup_read(): called to setup ADC registers for next measurment,
|
||||
* from interrupt */
|
||||
void setup_read();
|
||||
|
||||
/* stop_read(): called to stop device measurement */
|
||||
void stop_read();
|
||||
|
||||
/* reading_settled(): called to check if we have read for long enough */
|
||||
bool reading_settled();
|
||||
|
||||
/* read_average: called to calculate and clear the internal average.
|
||||
* implements read_average(), unscaled. */
|
||||
float _read_average();
|
||||
|
||||
int16_t get_pin() { return _pin; };
|
||||
private:
|
||||
/* following three are used from both an interrupt and normal thread */
|
||||
volatile uint8_t _sum_count;
|
||||
volatile uint16_t _sum;
|
||||
volatile uint16_t _latest;
|
||||
float _last_average;
|
||||
|
||||
/* _pin designates the ADC input mux for the sample */
|
||||
uint8_t _pin;
|
||||
|
||||
/* _stop_pin designates a digital pin to use for
|
||||
enabling/disabling the analog device */
|
||||
uint8_t _stop_pin;
|
||||
uint16_t _settle_time_ms;
|
||||
uint32_t _read_start_time_ms;
|
||||
};
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEAnalogIn : public AP_HAL::AnalogIn {
|
||||
public:
|
||||
FLYMAPLEAnalogIn();
|
||||
void init(void* implspecific);
|
||||
AP_HAL::AnalogSource* channel(int16_t n);
|
||||
|
||||
protected:
|
||||
static FLYMAPLEAnalogSource* _create_channel(int16_t num);
|
||||
static void _register_channel(FLYMAPLEAnalogSource*);
|
||||
static void _timer_event(uint32_t);
|
||||
static FLYMAPLEAnalogSource* _channels[FLYMAPLE_INPUT_MAX_CHANNELS];
|
||||
static int16_t _num_channels;
|
||||
static int16_t _active_channel;
|
||||
static uint16_t _channel_repeat_count;
|
||||
|
||||
private:
|
||||
// On Flymaple, VCC measurement is at pin 20. VCC (=VIN) of 5V is only present
|
||||
// if external voltage (not USB) is connected. Also there is a voltage
|
||||
// divider (25k/5k)
|
||||
FLYMAPLEAnalogSource _vcc;
|
||||
};
|
||||
#endif // __AP_HAL_FLYMAPLE_ANALOGIN_H__
|
||||
|
|
@ -0,0 +1,182 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "FlymapleWirish.h"
|
||||
#include "AnalogIn.h"
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
FLYMAPLEAnalogSource::FLYMAPLEAnalogSource(uint8_t pin) :
|
||||
_sum_count(0),
|
||||
_sum(0),
|
||||
_last_average(0),
|
||||
_pin(ANALOG_INPUT_NONE),
|
||||
_stop_pin(ANALOG_INPUT_NONE),
|
||||
_settle_time_ms(0)
|
||||
{
|
||||
set_pin(pin);
|
||||
}
|
||||
|
||||
float FLYMAPLEAnalogSource::read_average() {
|
||||
return _read_average();
|
||||
}
|
||||
|
||||
float FLYMAPLEAnalogSource::read_latest() {
|
||||
noInterrupts();
|
||||
uint16_t latest = _latest;
|
||||
interrupts();
|
||||
return latest;
|
||||
}
|
||||
|
||||
/*
|
||||
return voltage from 0.0 to 3.3V, scaled to Vcc
|
||||
*/
|
||||
float FLYMAPLEAnalogSource::voltage_average(void)
|
||||
{
|
||||
return voltage_average_ratiometric();
|
||||
}
|
||||
|
||||
float FLYMAPLEAnalogSource::voltage_latest(void)
|
||||
{
|
||||
float v = read_latest();
|
||||
return v * (3.3f / 4095.0f);
|
||||
}
|
||||
|
||||
/*
|
||||
return voltage from 0.0 to 3.3V, assuming a ratiometric sensor. This
|
||||
means the result is really a pseudo-voltage, that assumes the supply
|
||||
voltage is exactly 3.3V.
|
||||
*/
|
||||
float FLYMAPLEAnalogSource::voltage_average_ratiometric(void)
|
||||
{
|
||||
float v = read_average();
|
||||
return v * (3.3f / 4095.0f);
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogSource::set_pin(uint8_t pin) {
|
||||
if (pin != _pin) {
|
||||
// ensure the pin is marked as an INPUT pin
|
||||
if (pin != ANALOG_INPUT_NONE && pin != ANALOG_INPUT_BOARD_VCC) {
|
||||
int8_t dpin = hal.gpio->analogPinToDigitalPin(pin);
|
||||
if (dpin != -1) {
|
||||
pinMode(dpin, INPUT_ANALOG);
|
||||
}
|
||||
}
|
||||
noInterrupts();
|
||||
_sum = 0;
|
||||
_sum_count = 0;
|
||||
_last_average = 0;
|
||||
_latest = 0;
|
||||
_pin = pin;
|
||||
interrupts();
|
||||
}
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogSource::set_stop_pin(uint8_t pin) {
|
||||
_stop_pin = pin;
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogSource::set_settle_time(uint16_t settle_time_ms)
|
||||
{
|
||||
_settle_time_ms = settle_time_ms;
|
||||
}
|
||||
|
||||
/* read_average is called from the normal thread (not an interrupt). */
|
||||
float FLYMAPLEAnalogSource::_read_average()
|
||||
{
|
||||
uint16_t sum;
|
||||
uint8_t sum_count;
|
||||
|
||||
if (_sum_count == 0) {
|
||||
// avoid blocking waiting for new samples
|
||||
return _last_average;
|
||||
}
|
||||
|
||||
/* Read and clear in a critical section */
|
||||
noInterrupts();
|
||||
sum = _sum;
|
||||
sum_count = _sum_count;
|
||||
_sum = 0;
|
||||
_sum_count = 0;
|
||||
|
||||
interrupts();
|
||||
float avg = sum / (float) sum_count;
|
||||
|
||||
_last_average = avg;
|
||||
return avg;
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogSource::setup_read() {
|
||||
if (_stop_pin != ANALOG_INPUT_NONE) {
|
||||
uint8_t digital_pin = hal.gpio->analogPinToDigitalPin(_stop_pin);
|
||||
hal.gpio->pinMode(digital_pin, GPIO_OUTPUT);
|
||||
hal.gpio->write(digital_pin, 1);
|
||||
}
|
||||
if (_settle_time_ms != 0) {
|
||||
_read_start_time_ms = hal.scheduler->millis();
|
||||
}
|
||||
adc_reg_map *regs = ADC1->regs;
|
||||
adc_set_reg_seqlen(ADC1, 1);
|
||||
uint8 channel = 0;
|
||||
if (_pin == ANALOG_INPUT_BOARD_VCC)
|
||||
channel = PIN_MAP[FLYMAPLE_VCC_ANALOG_IN_PIN].adc_channel;
|
||||
else if (_pin == ANALOG_INPUT_NONE)
|
||||
; // NOOP
|
||||
else
|
||||
channel = PIN_MAP[_pin].adc_channel;
|
||||
regs->SQR3 = channel;
|
||||
}
|
||||
|
||||
void FLYMAPLEAnalogSource::stop_read() {
|
||||
if (_stop_pin != ANALOG_INPUT_NONE) {
|
||||
uint8_t digital_pin = hal.gpio->analogPinToDigitalPin(_stop_pin);
|
||||
hal.gpio->pinMode(digital_pin, GPIO_OUTPUT);
|
||||
hal.gpio->write(digital_pin, 0);
|
||||
}
|
||||
}
|
||||
|
||||
bool FLYMAPLEAnalogSource::reading_settled()
|
||||
{
|
||||
if (_settle_time_ms != 0 && (hal.scheduler->millis() - _read_start_time_ms) < _settle_time_ms) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* new_sample is called from an interrupt. It always has access to
|
||||
* _sum and _sum_count. Lock out the interrupts briefly with
|
||||
* noInterrupts()/interrupts() to read these variables from outside an interrupt. */
|
||||
void FLYMAPLEAnalogSource::new_sample(uint16_t sample) {
|
||||
_sum += sample;
|
||||
_latest = sample;
|
||||
// Copied from AVR code in ArduPlane-2.74b, but AVR code is wrong!
|
||||
if (_sum_count >= 15) { // Flymaple has a 12 bit ADC, so can only sum 16 in a uint16_t
|
||||
_sum >>= 1;
|
||||
_sum_count = 8;
|
||||
} else {
|
||||
_sum_count++;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include <stdarg.h>
|
||||
#include "Console.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
// Flymaple: Force init to be called *first*, i.e. before static object allocation.
|
||||
// Otherwise, statically allocated objects (eg SerialUSB) that need libmaple may fail.
|
||||
__attribute__((constructor)) void premain() {
|
||||
init();
|
||||
}
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
FLYMAPLEConsoleDriver::FLYMAPLEConsoleDriver(void* notused)
|
||||
{}
|
||||
|
||||
void FLYMAPLEConsoleDriver::init(void* machtnichts)
|
||||
{
|
||||
}
|
||||
|
||||
void FLYMAPLEConsoleDriver::backend_open()
|
||||
{}
|
||||
|
||||
void FLYMAPLEConsoleDriver::backend_close()
|
||||
{}
|
||||
|
||||
size_t FLYMAPLEConsoleDriver::backend_read(uint8_t *data, size_t len) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
size_t FLYMAPLEConsoleDriver::backend_write(const uint8_t *data, size_t len) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int16_t FLYMAPLEConsoleDriver::available() {
|
||||
return SerialUSB.available();
|
||||
}
|
||||
|
||||
int16_t FLYMAPLEConsoleDriver::txspace() {
|
||||
// REVISIT mikem
|
||||
return 0;
|
||||
}
|
||||
|
||||
int16_t FLYMAPLEConsoleDriver::read() {
|
||||
return SerialUSB.read();
|
||||
}
|
||||
|
||||
size_t FLYMAPLEConsoleDriver::write(uint8_t c) {
|
||||
SerialUSB.write(c);
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
|
||||
#ifndef __AP_HAL_FLYMAPLE_CONSOLE_H__
|
||||
#define __AP_HAL_FLYMAPLE_CONSOLE_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEConsoleDriver : public AP_HAL::ConsoleDriver {
|
||||
public:
|
||||
FLYMAPLEConsoleDriver(void* notused);
|
||||
void init(void* machtnichts);
|
||||
void backend_open();
|
||||
void backend_close();
|
||||
size_t backend_read(uint8_t *data, size_t len);
|
||||
size_t backend_write(const uint8_t *data, size_t len);
|
||||
|
||||
int16_t available();
|
||||
int16_t txspace();
|
||||
int16_t read();
|
||||
|
||||
size_t write(uint8_t c);
|
||||
private:
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_CONSOLE_H__
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
2013-09-23
|
||||
|
||||
These notes describe the steps take to port ArduPilot to the Flymaple platform
|
||||
http://www.open-drone.org/flymaple
|
||||
|
||||
Flymaple has an ARM based Cortex-3 STM32F103RE, 72MHz processor with 10DOF builtin sensors
|
||||
|
||||
|
||||
Implementation
|
||||
|
||||
Unlike the Arduino versions of ArduPlane, the Flymaple port uses portions of
|
||||
the libmaple library, including the stm32f1 core and some other libaries like
|
||||
Wire, HardwareTimer, HArdwareSPI etc.
|
||||
|
||||
Most of the changes are confined to new directory libraries/AP_HAL_FLYMAPLE
|
||||
which contains the Flymaple specific code. All the HAL modules have
|
||||
been ported, sometimes based on HAL_AVR, sometimes HAL_PX4:
|
||||
AnalogIn
|
||||
AnalogSource
|
||||
Console
|
||||
GPIO
|
||||
I2CDriver
|
||||
RCInput
|
||||
RCOoutput
|
||||
Scheduler
|
||||
Semaphores
|
||||
SPIDriver
|
||||
Storage
|
||||
UARTDriver
|
||||
Utility
|
||||
|
||||
The implementation of Storage uses EEPROM emulation code that uses 2 pages of
|
||||
Flymaple FLASH ROM as EEPROM. It was copied from
|
||||
AeroQuad_v3.2 to libraries/AP_HAL_FLYMAPLE/utility and slightly modified:
|
||||
libraries/AP_HAL_FLYMAPLE/utility/EEPROM.*
|
||||
libraries/AP_HAL_FLYMAPLE/utility/flash_stm32.*
|
||||
|
||||
Unlike other HAL ports, the namespace for Flymaple defined in
|
||||
AP_HAL_FLYMAPLE_Namespace.h has a more extensive name: 'AP_HAL_FLYMAPLE_NS' else
|
||||
get complaints from gcc 4.4.1 caused by collisions with other class names.
|
||||
|
||||
New board makefile mk/board/flymaple.mk, plus some other minor changes in
|
||||
mk/*.mk
|
||||
|
||||
In other parts of the ArduPlane tree, the changes have been #ifdefed for
|
||||
Flymaple except:
|
||||
|
||||
- ArduPlane/compat.pde
|
||||
All the wiring compatibility functions have been inline-d, else get
|
||||
contamination of the namespace at link time (multiple definitions of delay()
|
||||
etc)
|
||||
|
||||
- libraries/AP_Compass/AP_Compass_HMC5843.cpp
|
||||
- libraries/AP_Compass/Compass.h
|
||||
- libraries/AP_Baro/AP_Baro_BMP085.cpp
|
||||
Minor changes to raw data fetches to make them 32bit compatible. Should not
|
||||
affect other platforms.
|
||||
|
||||
Some other minor edits to eliminate compiler warnings
|
||||
|
||||
Resource usage
|
||||
Resources on the Flymaple board have been allocated by the HAL:
|
||||
|
||||
Pins
|
||||
0 GPS Rx in
|
||||
6 Receiver PPM in
|
||||
7 GCS Rx in
|
||||
8 GCS Tx out
|
||||
15 3.3V board VCC analog in
|
||||
16 Airspeed analog in (if available)
|
||||
19 Battery current analog in (if available)
|
||||
20 Battery voltage analog in (on-board divider connected to board VIN)
|
||||
|
||||
Timers
|
||||
SysTick 100Hz normal timers
|
||||
1 RCInput
|
||||
2 Failsafe timer
|
||||
3,4 RCOut
|
||||
8 not used
|
||||
|
||||
Installation on Linux
|
||||
|
||||
Tested with:
|
||||
libmaple https://github.com/leaflabs/libmaple http://leaflabs.com/docs/unix-toolchain.html
|
||||
arm-none-eabi-g++ toolchain, version 4.4.1
|
||||
on OpenSuSE 12.3
|
||||
Mission Planner 1.2.78
|
||||
|
||||
You need a number of additional resources to build ArduPlane for Flymaple. I
|
||||
have assumed that you will install them in your home directory, but they can really
|
||||
go anywhere provided you make the appropriate changes
|
||||
|
||||
cd ~
|
||||
git clone https://github.com/leaflabs/libmaple.git
|
||||
cd libmaple
|
||||
wget http://static.leaflabs.com/pub/codesourcery/gcc-arm-none-eabi-latest-linux32.tar.gz
|
||||
tar xvzf gcc-arm-none-eabi-latest-linux32.tar.gz
|
||||
export PATH=$PATH:~/libmaple/arm/bin
|
||||
|
||||
edit ArduPlane/config.mk to be something like:
|
||||
#config.mk START
|
||||
# Select maple_RET6 for Flymaple
|
||||
BOARD = maple_RET6
|
||||
# HAL_BOARD determines default HAL target.
|
||||
HAL_BOARD ?= HAL_BOARD_FLYMAPLE
|
||||
# The communication port used to communicate with the Flymaple
|
||||
PORT = /dev/ttyACM0
|
||||
# You must provide the path to the libmaple library directory:
|
||||
LIBMAPLE_PATH = $(HOME)/libmaple
|
||||
# Also, the ARM compiler tools MUST be in your current PATH
|
||||
#config.mk END
|
||||
|
||||
Remaining issues:
|
||||
|
||||
1. For reasons I do not yet understand, the magnetic heading reported by
|
||||
ArduPlance is 90 degrees away from what I think it should be.
|
||||
The sensors on the Flymaple are all aligned with the nominal X, Y and Z axes
|
||||
collinear. Pitch and roll axes point to the _corners_ of the board, not the
|
||||
flat sides as might be considered normal for a sensor board.
|
||||
Orientation is set to NONE, and yes, the reading of compass sensor registers
|
||||
is in teh right order for the HMC5883.
|
||||
|
||||
2. Many alignment warnings emitted by the compiler from libraries/GCS_MAVLink
|
||||
protocol.h eg:
|
||||
mnt/disk2/src/ArduPlane-2.74b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/../protocol.h: In function 'uint16_t _MAV_RETURN_uint16_t(const mavlink_message_t*, uint8_t)':
|
||||
/mnt/disk2/src/ArduPlane-2.74b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/../protocol.h:267: warning: cast from 'const char*' to 'const uint16_t*' increases required alignment of target type
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
// FlymapleWirish.h
|
||||
// Wrapper around wirish.h
|
||||
// to prevent various conflicts and problems with
|
||||
// raw include
|
||||
|
||||
#undef true
|
||||
#undef false
|
||||
|
||||
#include <wirish.h>
|
||||
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "GPIO.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
// Glue into libmaple
|
||||
void libmaple_pinMode(uint8_t pin, uint8_t output)
|
||||
{
|
||||
WiringPinMode mode = output ? OUTPUT : INPUT;
|
||||
pinMode(pin, mode);
|
||||
}
|
||||
|
||||
uint8_t libmaple_digitalRead(uint8_t pin)
|
||||
{
|
||||
return digitalRead(pin);
|
||||
}
|
||||
void libmaple_digitalWrite(uint8_t pin, uint8_t value)
|
||||
{
|
||||
digitalWrite(pin, value);
|
||||
}
|
||||
|
||||
FLYMAPLEGPIO::FLYMAPLEGPIO()
|
||||
{}
|
||||
|
||||
void FLYMAPLEGPIO::init()
|
||||
{}
|
||||
|
||||
void FLYMAPLEGPIO::pinMode(uint8_t pin, uint8_t output)
|
||||
{
|
||||
return libmaple_pinMode(pin, output);
|
||||
}
|
||||
|
||||
int8_t FLYMAPLEGPIO::analogPinToDigitalPin(uint8_t pin)
|
||||
{
|
||||
return pin;
|
||||
}
|
||||
|
||||
|
||||
uint8_t FLYMAPLEGPIO::read(uint8_t pin)
|
||||
{
|
||||
return libmaple_digitalRead(pin);
|
||||
}
|
||||
|
||||
void FLYMAPLEGPIO::write(uint8_t pin, uint8_t value)
|
||||
{
|
||||
libmaple_digitalWrite(pin, value);
|
||||
}
|
||||
|
||||
void FLYMAPLEGPIO::toggle(uint8_t pin)
|
||||
{
|
||||
libmaple_digitalWrite(pin, !libmaple_digitalRead(pin));
|
||||
}
|
||||
|
||||
/* Alternative interface: */
|
||||
AP_HAL::DigitalSource* FLYMAPLEGPIO::channel(uint16_t n) {
|
||||
return new FLYMAPLEDigitalSource(n);
|
||||
}
|
||||
|
||||
/* Interrupt interface: */
|
||||
bool FLYMAPLEGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode)
|
||||
{
|
||||
// Flymaple can only handle RISING, FALLING and CHANGE
|
||||
ExtIntTriggerMode flymaple_interrupt_mode;
|
||||
if (mode == GPIO_INTERRUPT_FALLING)
|
||||
flymaple_interrupt_mode = FALLING;
|
||||
else if (mode == GPIO_INTERRUPT_RISING)
|
||||
flymaple_interrupt_mode = RISING;
|
||||
else
|
||||
return false;
|
||||
|
||||
// REVISIT: Assumes pin and interrupt number are the same. Are they?
|
||||
attachInterrupt(interrupt_num, p, flymaple_interrupt_mode);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FLYMAPLEGPIO::usb_connected(void)
|
||||
{
|
||||
return SerialUSB.isConnected();
|
||||
}
|
||||
|
||||
FLYMAPLEDigitalSource::FLYMAPLEDigitalSource(uint8_t v) :
|
||||
_v(v)
|
||||
{
|
||||
SerialUSB.println(_v);
|
||||
}
|
||||
|
||||
void FLYMAPLEDigitalSource::mode(uint8_t output)
|
||||
{
|
||||
libmaple_pinMode(_v, output);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEDigitalSource::read() {
|
||||
return libmaple_digitalRead(_v);
|
||||
}
|
||||
|
||||
void FLYMAPLEDigitalSource::write(uint8_t value) {
|
||||
libmaple_digitalWrite(_v, value);
|
||||
}
|
||||
|
||||
void FLYMAPLEDigitalSource::toggle() {
|
||||
libmaple_digitalWrite(_v, !libmaple_digitalRead(_v));
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_GPIO_H__
|
||||
#define __AP_HAL_FLYMAPLE_GPIO_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEGPIO : public AP_HAL::GPIO {
|
||||
public:
|
||||
FLYMAPLEGPIO();
|
||||
void init();
|
||||
void pinMode(uint8_t pin, uint8_t output);
|
||||
int8_t analogPinToDigitalPin(uint8_t pin);
|
||||
uint8_t read(uint8_t pin);
|
||||
void write(uint8_t pin, uint8_t value);
|
||||
void toggle(uint8_t pin);
|
||||
|
||||
/* Alternative interface: */
|
||||
AP_HAL::DigitalSource* channel(uint16_t n);
|
||||
|
||||
/* Interrupt interface: */
|
||||
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
|
||||
uint8_t mode);
|
||||
|
||||
/* return true if USB cable is connected */
|
||||
bool usb_connected(void);
|
||||
|
||||
};
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEDigitalSource : public AP_HAL::DigitalSource {
|
||||
public:
|
||||
FLYMAPLEDigitalSource(uint8_t v);
|
||||
void mode(uint8_t output);
|
||||
uint8_t read();
|
||||
void write(uint8_t value);
|
||||
void toggle();
|
||||
|
||||
private:
|
||||
uint8_t _v;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_GPIO_H__
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "HAL_FLYMAPLE_Class.h"
|
||||
#include "AP_HAL_FLYMAPLE_Private.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
class HardwareSerial;
|
||||
extern HardwareSerial Serial1;
|
||||
extern HardwareSerial Serial2;
|
||||
extern HardwareSerial Serial3;
|
||||
|
||||
static FLYMAPLEUARTDriver uartADriver(&Serial1); // "COM1"
|
||||
static FLYMAPLEUARTDriver uartBDriver(&Serial2);
|
||||
static FLYMAPLEUARTDriver uartCDriver(&Serial3); // "GPS"
|
||||
static FLYMAPLESemaphore i2cSemaphore;
|
||||
static FLYMAPLEI2CDriver i2cDriver(&i2cSemaphore);
|
||||
static FLYMAPLESPIDeviceManager spiDeviceManager;
|
||||
static FLYMAPLEAnalogIn analogIn;
|
||||
static FLYMAPLEStorage storageDriver;
|
||||
static FLYMAPLEConsoleDriver consoleDriver(&uartADriver);
|
||||
static FLYMAPLEGPIO gpioDriver;
|
||||
static FLYMAPLERCInput rcinDriver;
|
||||
static FLYMAPLERCOutput rcoutDriver;
|
||||
static FLYMAPLEScheduler schedulerInstance;
|
||||
static FLYMAPLEUtil utilInstance;
|
||||
|
||||
HAL_FLYMAPLE::HAL_FLYMAPLE() :
|
||||
AP_HAL::HAL(
|
||||
&uartADriver,
|
||||
&uartBDriver,
|
||||
&uartCDriver,
|
||||
&i2cDriver,
|
||||
&spiDeviceManager,
|
||||
&analogIn,
|
||||
&storageDriver,
|
||||
&consoleDriver,
|
||||
&gpioDriver,
|
||||
&rcinDriver,
|
||||
&rcoutDriver,
|
||||
&schedulerInstance,
|
||||
&utilInstance
|
||||
)
|
||||
{}
|
||||
|
||||
void HAL_FLYMAPLE::init(int argc,char* const argv[]) const {
|
||||
/* initialize all drivers and private members here.
|
||||
* up to the programmer to do this in the correct order.
|
||||
* Scheduler should likely come first. */
|
||||
scheduler->init(NULL);
|
||||
console->init(NULL);
|
||||
|
||||
/* The AVR RCInput drivers take an AP_HAL_AVR::ISRRegistry*
|
||||
* as the init argument */
|
||||
rcin->init(NULL);
|
||||
rcout->init(NULL);
|
||||
spi->init(NULL);
|
||||
i2c->begin();
|
||||
i2c->setTimeout(100);
|
||||
analogin->init(NULL);
|
||||
storage->init(NULL); // Uses EEPROM.*, flash_stm* copied from AeroQuad_v3.2
|
||||
}
|
||||
|
||||
const HAL_FLYMAPLE AP_HAL_FLYMAPLE;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_CLASS_H__
|
||||
#define __AP_HAL_FLYMAPLE_CLASS_H__
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#include "AP_HAL_FLYMAPLE_Namespace.h"
|
||||
|
||||
class HAL_FLYMAPLE : public AP_HAL::HAL {
|
||||
public:
|
||||
HAL_FLYMAPLE();
|
||||
void init(int argc, char * const * argv) const;
|
||||
};
|
||||
|
||||
extern const HAL_FLYMAPLE AP_HAL_FLYMAPLE;
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_CLASS_H__
|
||||
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
// Flymaple I2CDriver
|
||||
|
||||
#include "I2CDriver.h"
|
||||
#include "FlymapleWirish.h"
|
||||
#include <Wire.h>
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
static TwoWire twowire(5, 9); // Flymaple has non-standard SCL, SDA
|
||||
void FLYMAPLEI2CDriver::begin()
|
||||
{
|
||||
twowire.begin();
|
||||
}
|
||||
|
||||
void FLYMAPLEI2CDriver::end() {}
|
||||
|
||||
void FLYMAPLEI2CDriver::setTimeout(uint16_t ms) {}
|
||||
void FLYMAPLEI2CDriver::setHighSpeed(bool active) {}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
|
||||
{
|
||||
twowire.beginTransmission(addr);
|
||||
twowire.send(data, len);
|
||||
uint8_t result = twowire.endTransmission();
|
||||
return result;
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
|
||||
{
|
||||
return writeRegisters(addr, reg, 1, &val);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
|
||||
uint8_t len, uint8_t* data)
|
||||
{
|
||||
twowire.beginTransmission(addr);
|
||||
twowire.send(reg);
|
||||
if (len)
|
||||
twowire.send(data, len);
|
||||
uint8_t result = twowire.endTransmission();
|
||||
return result;
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
|
||||
{
|
||||
uint8_t actual_len = twowire.requestFrom(addr, len);
|
||||
for (uint8_t i = 0; i < actual_len; i++)
|
||||
*data++ = twowire.receive();
|
||||
return actual_len != len;
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
|
||||
{
|
||||
return readRegisters(addr, reg, 1, data);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
|
||||
uint8_t len, uint8_t* data)
|
||||
{
|
||||
writeRegisters(addr, reg, 0, NULL); // Tell device which register we want
|
||||
return read(addr, len, data);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEI2CDriver::lockup_count() {return 0;}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_I2CDRIVER_H__
|
||||
#define __AP_HAL_FLYMAPLE_I2CDRIVER_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEI2CDriver : public AP_HAL::I2CDriver {
|
||||
public:
|
||||
FLYMAPLEI2CDriver(AP_HAL::Semaphore* semaphore) : _semaphore(semaphore) {}
|
||||
void begin();
|
||||
void end();
|
||||
void setTimeout(uint16_t ms);
|
||||
void setHighSpeed(bool active);
|
||||
|
||||
/* write: for i2c devices which do not obey register conventions */
|
||||
uint8_t write(uint8_t addr, uint8_t len, uint8_t* data);
|
||||
/* writeRegister: write a single 8-bit value to a register */
|
||||
uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val);
|
||||
/* writeRegisters: write bytes to contigious registers */
|
||||
uint8_t writeRegisters(uint8_t addr, uint8_t reg,
|
||||
uint8_t len, uint8_t* data);
|
||||
|
||||
/* read: for i2c devices which do not obey register conventions */
|
||||
uint8_t read(uint8_t addr, uint8_t len, uint8_t* data);
|
||||
/* readRegister: read from a device register - writes the register,
|
||||
* then reads back an 8-bit value. */
|
||||
uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data);
|
||||
/* readRegister: read contigious device registers - writes the first
|
||||
* register, then reads back multiple bytes */
|
||||
uint8_t readRegisters(uint8_t addr, uint8_t reg,
|
||||
uint8_t len, uint8_t* data);
|
||||
|
||||
uint8_t lockup_count();
|
||||
|
||||
AP_HAL::Semaphore* get_semaphore() { return _semaphore; }
|
||||
|
||||
private:
|
||||
AP_HAL::Semaphore* _semaphore;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_I2CDRIVER_H__
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
// Flymaple RCInput
|
||||
// PPM input from a single pin
|
||||
|
||||
#include "RCInput.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
/* private variables to communicate with input capture isr */
|
||||
volatile uint16_t FLYMAPLERCInput::_pulse_capt[FLYMAPLE_RC_INPUT_NUM_CHANNELS] = {0};
|
||||
volatile uint8_t FLYMAPLERCInput::_valid_channels = 0;
|
||||
|
||||
#define FLYMAPLE_RC_INPUT_PIN 6
|
||||
|
||||
FLYMAPLERCInput::FLYMAPLERCInput()
|
||||
{}
|
||||
|
||||
void FLYMAPLERCInput::_timer_capt_cb(void)
|
||||
{
|
||||
static int on = 0;
|
||||
on = !on;
|
||||
digitalWrite(13, on);
|
||||
|
||||
static uint16 previous_count;
|
||||
static uint8 channel_ctr;
|
||||
|
||||
// Read the CCR register, where the time count since the last input pin transition will be
|
||||
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
|
||||
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
|
||||
uint16 current_count = timer_get_compare(tdev, timer_channel);
|
||||
uint32 sr = (tdev->regs).gen->SR;
|
||||
uint32 overcapture_mask = (1 << (TIMER_SR_CC1OF_BIT + timer_channel - 1));
|
||||
bool overcapture = sr & overcapture_mask;
|
||||
|
||||
uint16_t pulse_width;
|
||||
if (current_count < previous_count) {
|
||||
/* counter rolls over at 40000 */
|
||||
pulse_width = current_count + 40000 - previous_count;
|
||||
} else {
|
||||
pulse_width = current_count - previous_count;
|
||||
}
|
||||
|
||||
if (overcapture)
|
||||
(tdev->regs).gen->SR &= ~overcapture_mask; // Clear overcapture mask
|
||||
|
||||
if (overcapture || pulse_width > 8000) {
|
||||
// sync pulse detected. Pass through values if at least a minimum number of channels received
|
||||
if( channel_ctr >= FLYMAPLE_RC_INPUT_MIN_CHANNELS ) {
|
||||
_valid_channels = channel_ctr;
|
||||
}
|
||||
channel_ctr = 0;
|
||||
} else {
|
||||
if (channel_ctr < FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
|
||||
_pulse_capt[channel_ctr] = pulse_width;
|
||||
channel_ctr++;
|
||||
if (channel_ctr == FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
|
||||
_valid_channels = FLYMAPLE_RC_INPUT_NUM_CHANNELS;
|
||||
}
|
||||
}
|
||||
}
|
||||
previous_count = current_count;
|
||||
|
||||
}
|
||||
|
||||
void FLYMAPLERCInput::init(void* machtnichts)
|
||||
{
|
||||
/* initialize overrides */
|
||||
clear_overrides();
|
||||
|
||||
// Configure pin 6 input to timer 1 CH1 bRin Input Capture mode
|
||||
pinMode(FLYMAPLE_RC_INPUT_PIN, INPUT_PULLDOWN);
|
||||
|
||||
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
|
||||
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
|
||||
timer_pause(tdev); // disabled
|
||||
timer_set_prescaler(tdev, (CYCLES_PER_MICROSECOND/2) - 1); // 2MHz = 0.5us timer ticks
|
||||
timer_set_reload(tdev, 40000);
|
||||
(tdev->regs).gen->CCMR1 = TIMER_CCMR1_CC1S_INPUT_TI1; // no prescaler, input from T1, no filter
|
||||
(tdev->regs).gen->CCER = TIMER_CCER_CC1P | TIMER_CCER_CC1E; // falling edge, enable capture
|
||||
timer_attach_interrupt(tdev, timer_channel, _timer_capt_cb);
|
||||
timer_generate_update(tdev);
|
||||
timer_resume(tdev); // reenabled
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLERCInput::valid_channels() {
|
||||
return _valid_channels;
|
||||
}
|
||||
|
||||
/* constrain captured pulse to be between min and max pulsewidth. */
|
||||
static inline uint16_t constrain_pulse(uint16_t p) {
|
||||
if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH;
|
||||
if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH;
|
||||
return p;
|
||||
}
|
||||
|
||||
uint16_t FLYMAPLERCInput::read(uint8_t ch) {
|
||||
/* constrain ch */
|
||||
if (ch >= FLYMAPLE_RC_INPUT_NUM_CHANNELS)
|
||||
return 0;
|
||||
/* grab channel from isr's memory in critical section*/
|
||||
noInterrupts();
|
||||
uint16_t capt = _pulse_capt[ch];
|
||||
interrupts();
|
||||
_valid_channels = 0;
|
||||
/* scale _pulse_capt from 0.5us units to 1us units. */
|
||||
uint16_t pulse = constrain_pulse(capt >> 1);
|
||||
/* Check for override */
|
||||
uint16_t over = _override[ch];
|
||||
return (over == 0) ? pulse : over;
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLERCInput::read(uint16_t* periods, uint8_t len) {
|
||||
/* constrain len */
|
||||
if (len > FLYMAPLE_RC_INPUT_NUM_CHANNELS)
|
||||
len = FLYMAPLE_RC_INPUT_NUM_CHANNELS;
|
||||
/* grab channels from isr's memory in critical section */
|
||||
noInterrupts();
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
periods[i] = _pulse_capt[i];
|
||||
}
|
||||
interrupts();
|
||||
/* Outside of critical section, do the math (in place) to scale and
|
||||
* constrain the pulse. */
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
/* scale _pulse_capt from 0.5us units to 1us units. */
|
||||
periods[i] = constrain_pulse(periods[i] >> 1);
|
||||
/* check for override */
|
||||
if (_override[i] != 0) {
|
||||
periods[i] = _override[i];
|
||||
}
|
||||
}
|
||||
uint8_t v = _valid_channels;
|
||||
_valid_channels = 0;
|
||||
return v;
|
||||
}
|
||||
|
||||
bool FLYMAPLERCInput::set_overrides(int16_t *overrides, uint8_t len) {
|
||||
bool res = false;
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
res |= set_override(i, overrides[i]);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
bool FLYMAPLERCInput::set_override(uint8_t channel, int16_t override) {
|
||||
if (override < 0) return false; /* -1: no change. */
|
||||
if (channel < FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
|
||||
_override[channel] = override;
|
||||
if (override != 0) {
|
||||
_valid_channels = 1;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void FLYMAPLERCInput::clear_overrides()
|
||||
{
|
||||
for (uint8_t i = 0; i < FLYMAPLE_RC_INPUT_NUM_CHANNELS; i++) {
|
||||
_override[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_RCINPUT_H__
|
||||
#define __AP_HAL_FLYMAPLE_RCINPUT_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
#define FLYMAPLE_RC_INPUT_NUM_CHANNELS 8
|
||||
|
||||
#define FLYMAPLE_RC_INPUT_MIN_CHANNELS 5 // for ppm sum we allow less than 8 channels to make up a valid packet
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLERCInput : public AP_HAL::RCInput {
|
||||
public:
|
||||
FLYMAPLERCInput();
|
||||
void init(void* machtnichts);
|
||||
uint8_t valid_channels();
|
||||
uint16_t read(uint8_t ch);
|
||||
uint8_t read(uint16_t* periods, uint8_t len);
|
||||
|
||||
bool set_overrides(int16_t *overrides, uint8_t len);
|
||||
bool set_override(uint8_t channel, int16_t override);
|
||||
void clear_overrides();
|
||||
|
||||
private:
|
||||
/* private callback for input capture ISR */
|
||||
static void _timer_capt_cb(void);
|
||||
|
||||
/* private variables to communicate with input capture isr */
|
||||
static volatile uint16_t _pulse_capt[FLYMAPLE_RC_INPUT_NUM_CHANNELS];
|
||||
static volatile uint8_t _valid_channels;
|
||||
|
||||
/* override state */
|
||||
uint16_t _override[FLYMAPLE_RC_INPUT_NUM_CHANNELS];
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_RCINPUT_H__
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
// Flymaple RC Outputs
|
||||
// Derived from libmaple Servo.cpp
|
||||
|
||||
#include "RCOutput.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
#define MAX_OVERFLOW ((1 << 16) - 1)
|
||||
|
||||
void FLYMAPLERCOutput::init(void* machtnichts) {}
|
||||
|
||||
void FLYMAPLERCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
||||
{
|
||||
for (int i = 0; i < 32; i++) {
|
||||
if ((chmask >> i) & 1) {
|
||||
_set_freq(i, freq_hz);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t FLYMAPLERCOutput::get_freq(uint8_t ch)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return 0;
|
||||
uint8_t pin = _channel_to_flymaple_pin(ch);
|
||||
timer_dev *tdev = PIN_MAP[pin].timer_device;
|
||||
|
||||
if (tdev == NULL)
|
||||
return 0; // Should never happen
|
||||
uint16 prescaler = timer_get_prescaler(tdev);
|
||||
uint16 overflow = timer_get_reload(tdev);
|
||||
return F_CPU / (prescaler+1) / overflow;
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::enable_ch(uint8_t ch)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return;
|
||||
uint8_t pin = _channel_to_flymaple_pin(ch);
|
||||
timer_dev *tdev = PIN_MAP[pin].timer_device;
|
||||
|
||||
if (tdev == NULL) {
|
||||
// don't reset any fields or ASSERT(0), to keep driving any
|
||||
// previously attach()ed servo.
|
||||
return;
|
||||
}
|
||||
pinMode(pin, PWM);
|
||||
_set_freq(ch, 50); // Default to 50 Hz
|
||||
write(ch, 0);
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::enable_mask(uint32_t chmask)
|
||||
{
|
||||
for (int i = 0; i < 32; i++) {
|
||||
if ((chmask >> i) & 1) {
|
||||
enable_ch(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::disable_ch(uint8_t ch)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return;
|
||||
// TODO
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::disable_mask(uint32_t chmask)
|
||||
{
|
||||
for (int i = 0; i < 32; i++) {
|
||||
if ((chmask >> i) & 1) {
|
||||
disable_ch(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::write(uint8_t ch, uint16_t period_us)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return;
|
||||
uint8_t pin = _channel_to_flymaple_pin(ch);
|
||||
pwmWrite(pin, (period_us * _clocks_per_msecond[ch]) / 1000);
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
|
||||
{
|
||||
for (int i = 0; i < len; i++)
|
||||
write(i + ch, period_us[i]);
|
||||
}
|
||||
|
||||
uint16_t FLYMAPLERCOutput::read(uint8_t ch)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return 0;
|
||||
uint8_t pin = _channel_to_flymaple_pin(ch);
|
||||
timer_dev *tdev = PIN_MAP[pin].timer_device;
|
||||
uint8 timer_channel = PIN_MAP[pin].timer_channel;
|
||||
__io uint32 *ccr = &(tdev->regs).gen->CCR1 + (timer_channel - 1);
|
||||
return *ccr * 1000 / _clocks_per_msecond[ch];
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::read(uint16_t* period_us, uint8_t len)
|
||||
{
|
||||
for (int i = 0; i < len; i++)
|
||||
period_us[i] = read(i);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLERCOutput::_channel_to_flymaple_pin(uint8_t ch)
|
||||
{
|
||||
// This maps the ArduPilot channel numbers to Flymaple PWM output pins
|
||||
// Channels on the same timer ALWAYS use the same frequency (the last one set)
|
||||
// 28, 27, 11, 12 use Timer 3 OK
|
||||
// 24, 14, 5, 9 use Timer 4 BREAKS I2C on pins 5 and 9
|
||||
// 35, 36, 37, 38 use Timer 8 DONT USE: CRASHES. WHY?
|
||||
// 0 1, 2, 3 use Timer 2 OK
|
||||
static uint8_t ch_to_pin[FLYMAPLE_RC_OUTPUT_NUM_CHANNELS] = { 28, 27, 11, 12, 24, 14 };
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return 0; // Should never happen. REVISIT?
|
||||
else
|
||||
return ch_to_pin[ch];
|
||||
}
|
||||
|
||||
void FLYMAPLERCOutput::_set_freq(uint8_t ch, uint16_t freq_hz)
|
||||
{
|
||||
if (ch >= FLYMAPLE_RC_OUTPUT_NUM_CHANNELS)
|
||||
return;
|
||||
if (freq_hz == 0)
|
||||
return; // Silly, avoid divide by 0 later
|
||||
uint8_t pin = _channel_to_flymaple_pin(ch);
|
||||
timer_dev *tdev = PIN_MAP[pin].timer_device;
|
||||
|
||||
if (tdev == NULL)
|
||||
return; // Should never happen
|
||||
|
||||
uint32 microseconds = 1000000 / freq_hz; // per period
|
||||
uint32 period_cyc = microseconds * CYCLES_PER_MICROSECOND; // system clock cycles per period
|
||||
// This picks the smallest prescaler that allows an overflow < 2^16.
|
||||
uint16 prescaler = (uint16)(period_cyc / MAX_OVERFLOW + 1);
|
||||
uint16 overflow = (uint16)(period_cyc / (prescaler+1));
|
||||
_clocks_per_msecond[ch] = F_CPU / (prescaler+1) / 1000;
|
||||
timer_pause(tdev);
|
||||
timer_set_prescaler(tdev, prescaler);
|
||||
timer_set_reload(tdev, overflow);
|
||||
timer_generate_update(tdev);
|
||||
timer_resume(tdev);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_RCOUTPUT_H__
|
||||
#define __AP_HAL_FLYMAPLE_RCOUTPUT_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
#define FLYMAPLE_RC_OUTPUT_NUM_CHANNELS 6
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLERCOutput : public AP_HAL::RCOutput {
|
||||
void init(void* machtnichts);
|
||||
void set_freq(uint32_t chmask, uint16_t freq_hz);
|
||||
uint16_t get_freq(uint8_t ch);
|
||||
void enable_ch(uint8_t ch);
|
||||
void enable_mask(uint32_t chmask);
|
||||
void disable_ch(uint8_t ch);
|
||||
void disable_mask(uint32_t chmask);
|
||||
void write(uint8_t ch, uint16_t period_us);
|
||||
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
|
||||
uint16_t read(uint8_t ch);
|
||||
void read(uint16_t* period_us, uint8_t len);
|
||||
|
||||
private:
|
||||
// We map channels to Flymaple pins so AP channels 1 to 6 map to
|
||||
// the conventional Flymaple PWM output pins on J5 (pins 28, 27, 11, 12, 24, 14 respectively)
|
||||
// also
|
||||
// Channel 7 -> pin 5
|
||||
// channel 8 -> pin 9
|
||||
uint8_t _channel_to_flymaple_pin(uint8_t ch);
|
||||
|
||||
void _set_freq(uint8_t ch, uint16_t freq_hz);
|
||||
|
||||
uint32_t _clocks_per_msecond[FLYMAPLE_RC_OUTPUT_NUM_CHANNELS];
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_RCOUTPUT_H__
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
// Flymaple SPI driver
|
||||
// Flymaple has no actual SPI devices, so this generic
|
||||
|
||||
#include "SPIDriver.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
#define FLYMAPLE_SPI_CS_PIN 8
|
||||
// Only one port so far:
|
||||
#define FLYMAPLE_SPI_PORT 1
|
||||
HardwareSPI spi(FLYMAPLE_SPI_PORT);
|
||||
|
||||
|
||||
FLYMAPLESPIDeviceDriver::FLYMAPLESPIDeviceDriver()
|
||||
{}
|
||||
|
||||
void FLYMAPLESPIDeviceDriver::init()
|
||||
{
|
||||
spi.begin(SPI_9MHZ, MSBFIRST, SPI_MODE_0);
|
||||
digitalWrite(FLYMAPLE_SPI_CS_PIN, 1);
|
||||
pinMode(FLYMAPLE_SPI_CS_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore* FLYMAPLESPIDeviceDriver::get_semaphore()
|
||||
{
|
||||
return &_semaphore;
|
||||
}
|
||||
|
||||
void FLYMAPLESPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len)
|
||||
{
|
||||
cs_assert();
|
||||
if (rx == NULL) {
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
transfer(tx[i]);
|
||||
}
|
||||
} else {
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
rx[i] = transfer(tx[i]);
|
||||
}
|
||||
}
|
||||
cs_release();
|
||||
}
|
||||
|
||||
|
||||
void FLYMAPLESPIDeviceDriver::cs_assert()
|
||||
{
|
||||
digitalWrite(FLYMAPLE_SPI_CS_PIN, 0);
|
||||
}
|
||||
|
||||
void FLYMAPLESPIDeviceDriver::cs_release()
|
||||
{
|
||||
digitalWrite(FLYMAPLE_SPI_CS_PIN, 1);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLESPIDeviceDriver::transfer (uint8_t data)
|
||||
{
|
||||
return spi.transfer(data);
|
||||
}
|
||||
|
||||
void FLYMAPLESPIDeviceDriver::transfer (const uint8_t *data, uint16_t len)
|
||||
{
|
||||
spi.write(data, len);
|
||||
}
|
||||
|
||||
FLYMAPLESPIDeviceManager::FLYMAPLESPIDeviceManager()
|
||||
{
|
||||
}
|
||||
|
||||
void FLYMAPLESPIDeviceManager::init(void *)
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL::SPIDeviceDriver* FLYMAPLESPIDeviceManager::device(enum AP_HAL::SPIDevice)
|
||||
{
|
||||
_device.init(); // Defer this until GPIO pin 13 is set up else its a slave
|
||||
return &_device;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_SPIDRIVER_H__
|
||||
#define __AP_HAL_FLYMAPLE_SPIDRIVER_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
#include "Semaphores.h"
|
||||
|
||||
class HardwareSPI; // in libmaple
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLESPIDeviceDriver : public AP_HAL::SPIDeviceDriver {
|
||||
public:
|
||||
FLYMAPLESPIDeviceDriver();
|
||||
void init();
|
||||
AP_HAL::Semaphore* get_semaphore();
|
||||
void transaction(const uint8_t *tx, uint8_t *rx, uint16_t len);
|
||||
|
||||
void cs_assert();
|
||||
void cs_release();
|
||||
uint8_t transfer (uint8_t data);
|
||||
void transfer (const uint8_t *data, uint16_t len);
|
||||
private:
|
||||
FLYMAPLESemaphore _semaphore;
|
||||
};
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLESPIDeviceManager : public AP_HAL::SPIDeviceManager {
|
||||
public:
|
||||
FLYMAPLESPIDeviceManager();
|
||||
void init(void *);
|
||||
AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDevice);
|
||||
private:
|
||||
FLYMAPLESPIDeviceDriver _device;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_SPIDRIVER_H__
|
||||
|
|
@ -0,0 +1,242 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
// Scheduler.cpp
|
||||
//
|
||||
// Flymaple Scheduler.
|
||||
// We use systick interrupt for the 1kHz ordinary timers.
|
||||
// We use a slightly higher priority HardwareTimer 2 for the failsafe callbacks
|
||||
// so a hung timer wont prevent the failsafe timer interrupt running
|
||||
|
||||
#include "Scheduler.h"
|
||||
|
||||
#define millis libmaple_millis
|
||||
#define micros libmaple_micros
|
||||
#include "FlymapleWirish.h"
|
||||
#undef millis
|
||||
#undef micros
|
||||
|
||||
// Not declared in any libmaple headers :-(
|
||||
extern "C"
|
||||
{
|
||||
void systick_attach_callback(void (*callback)(void));
|
||||
};
|
||||
|
||||
// Use Maple hardware timer for 1khz failsafe timer
|
||||
// Caution, this must agree with the interrupt number passed to
|
||||
// nvic_irq_set_priority
|
||||
static HardwareTimer _failsafe_timer(2);
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_HAL::TimedProc FLYMAPLEScheduler::_failsafe = NULL;
|
||||
volatile bool FLYMAPLEScheduler::_timer_suspended = false;
|
||||
volatile bool FLYMAPLEScheduler::_timer_event_missed = false;
|
||||
volatile bool FLYMAPLEScheduler::_in_timer_proc = false;
|
||||
AP_HAL::TimedProc FLYMAPLEScheduler::_timer_proc[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
||||
uint8_t FLYMAPLEScheduler::_num_timer_procs = 0;
|
||||
|
||||
FLYMAPLEScheduler::FLYMAPLEScheduler() :
|
||||
_delay_cb(NULL),
|
||||
_min_delay_cb_ms(65535),
|
||||
_initialized(false)
|
||||
{}
|
||||
|
||||
void FLYMAPLEScheduler::init(void* machtnichts)
|
||||
{
|
||||
delay_us(2000000); // Wait for startup so we have time to connect a new USB console
|
||||
// 1kHz interrupts from systick for normal timers
|
||||
systick_attach_callback(_timer_procs_timer_event);
|
||||
|
||||
// Set up Maple hardware timer for 1khz failsafe timer
|
||||
// ref: http://leaflabs.com/docs/lang/api/hardwaretimer.html#lang-hardwaretimer
|
||||
_failsafe_timer.pause();
|
||||
_failsafe_timer.setPeriod(1000); // 1000us = 1kHz
|
||||
_failsafe_timer.setChannelMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);// Set up an interrupt on channel 1
|
||||
_failsafe_timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
|
||||
_failsafe_timer.attachInterrupt(TIMER_CH1, _failsafe_timer_event);
|
||||
_failsafe_timer.refresh();// Refresh the timer's count, prescale, and overflow
|
||||
_failsafe_timer.resume(); // Start the timer counting
|
||||
// We run this timer at a higher priority, so that a broken timer handler (ie one that hangs)
|
||||
// will not prevent the failsafe timer interrupt.
|
||||
// Caution: the timer number must agree with the HardwareTimer number
|
||||
nvic_irq_set_priority(NVIC_TIMER2, 0x14);
|
||||
}
|
||||
|
||||
// This function may calls the _delay_cb to use up time
|
||||
void FLYMAPLEScheduler::delay(uint16_t ms)
|
||||
{
|
||||
uint32_t start = libmaple_micros();
|
||||
|
||||
while (ms > 0) {
|
||||
while ((libmaple_micros() - start) >= 1000) {
|
||||
ms--;
|
||||
if (ms == 0) break;
|
||||
start += 1000;
|
||||
}
|
||||
if (_min_delay_cb_ms <= ms) {
|
||||
if (_delay_cb) {
|
||||
_delay_cb();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t FLYMAPLEScheduler::millis() {
|
||||
return libmaple_millis();
|
||||
}
|
||||
|
||||
uint32_t FLYMAPLEScheduler::micros() {
|
||||
return libmaple_micros();
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::delay_microseconds(uint16_t us)
|
||||
{
|
||||
delay_us(us);
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms)
|
||||
{
|
||||
_delay_cb = proc;
|
||||
_min_delay_cb_ms = min_time_ms;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::register_timer_process(AP_HAL::TimedProc proc)
|
||||
{
|
||||
for (int i = 0; i < _num_timer_procs; i++) {
|
||||
if (_timer_proc[i] == proc) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (_num_timer_procs < FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS) {
|
||||
/* this write to _timer_proc can be outside the critical section
|
||||
* because that memory won't be used until _num_timer_procs is
|
||||
* incremented. */
|
||||
_timer_proc[_num_timer_procs] = proc;
|
||||
/* _num_timer_procs is used from interrupt, and multiple bytes long. */
|
||||
noInterrupts();
|
||||
_num_timer_procs++;
|
||||
interrupts();
|
||||
}
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::register_io_process(AP_HAL::TimedProc k)
|
||||
{
|
||||
// IO processes not supported on FLYMAPLE
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us)
|
||||
{
|
||||
/* XXX Assert period_us == 1000 */
|
||||
_failsafe = failsafe;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::suspend_timer_procs()
|
||||
{
|
||||
_timer_suspended = true;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::resume_timer_procs()
|
||||
{
|
||||
_timer_suspended = false;
|
||||
if (_timer_event_missed == true) {
|
||||
_run_timer_procs(false);
|
||||
_timer_event_missed = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool FLYMAPLEScheduler::in_timerprocess() {
|
||||
return _in_timer_proc;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::_timer_procs_timer_event() {
|
||||
_run_timer_procs(true);
|
||||
}
|
||||
|
||||
// Called by HardwareTimer when a failsafe timer event occurs
|
||||
void FLYMAPLEScheduler::_failsafe_timer_event()
|
||||
{
|
||||
// run the failsafe, if one is setup
|
||||
if (_failsafe != NULL)
|
||||
_failsafe(libmaple_micros());
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::begin_atomic()
|
||||
{
|
||||
noInterrupts();
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::end_atomic()
|
||||
{
|
||||
interrupts();
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::_run_timer_procs(bool called_from_isr)
|
||||
{
|
||||
uint32_t tnow = libmaple_micros();
|
||||
_in_timer_proc = true;
|
||||
|
||||
if (!_timer_suspended) {
|
||||
// now call the timer based drivers
|
||||
for (int i = 0; i < _num_timer_procs; i++) {
|
||||
if (_timer_proc[i] != NULL) {
|
||||
_timer_proc[i](tnow);
|
||||
}
|
||||
}
|
||||
} else if (called_from_isr) {
|
||||
_timer_event_missed = true;
|
||||
}
|
||||
|
||||
_in_timer_proc = false;
|
||||
}
|
||||
|
||||
bool FLYMAPLEScheduler::system_initializing() {
|
||||
return !_initialized;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::system_initialized()
|
||||
{
|
||||
if (_initialized) {
|
||||
panic(PSTR("PANIC: scheduler::system_initialized called"
|
||||
"more than once"));
|
||||
}
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::panic(const prog_char_t *errormsg) {
|
||||
/* Suspend timer processes. We still want the timer event to go off
|
||||
* to run the _failsafe code, however. */
|
||||
// REVISIT: not tested on FLYMAPLE
|
||||
_timer_suspended = true;
|
||||
hal.console->println_P(errormsg);
|
||||
for(;;);
|
||||
}
|
||||
|
||||
void FLYMAPLEScheduler::reboot(bool hold_in_bootloader) {
|
||||
hal.uartA->println_P(PSTR("GOING DOWN FOR A REBOOT\r\n"));
|
||||
hal.scheduler->delay(100);
|
||||
nvic_sys_reset();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_SCHEDULER_H__
|
||||
#define __AP_HAL_FLYMAPLE_SCHEDULER_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
#define FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS 4
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEScheduler : public AP_HAL::Scheduler {
|
||||
public:
|
||||
FLYMAPLEScheduler();
|
||||
void init(void* machtnichts);
|
||||
void delay(uint16_t ms);
|
||||
uint32_t millis();
|
||||
uint32_t micros();
|
||||
void delay_microseconds(uint16_t us);
|
||||
void register_delay_callback(AP_HAL::Proc,
|
||||
uint16_t min_time_ms);
|
||||
|
||||
void register_timer_process(AP_HAL::TimedProc);
|
||||
|
||||
void register_io_process(AP_HAL::TimedProc);
|
||||
|
||||
void suspend_timer_procs();
|
||||
void resume_timer_procs();
|
||||
|
||||
bool in_timerprocess();
|
||||
|
||||
void register_timer_failsafe(AP_HAL::TimedProc,
|
||||
uint32_t period_us);
|
||||
|
||||
void begin_atomic();
|
||||
void end_atomic();
|
||||
|
||||
bool system_initializing();
|
||||
void system_initialized();
|
||||
|
||||
void panic(const prog_char_t *errormsg);
|
||||
void reboot(bool hold_in_bootloader);
|
||||
|
||||
private:
|
||||
static volatile bool _in_timer_proc;
|
||||
|
||||
AP_HAL::Proc _delay_cb;
|
||||
uint16_t _min_delay_cb_ms;
|
||||
bool _initialized;
|
||||
|
||||
/* _timer_procs_timer_event() and _run_timer_procs are static so they can be
|
||||
* called from an interrupt. */
|
||||
static void _timer_procs_timer_event();
|
||||
static void _run_timer_procs(bool called_from_isr);
|
||||
|
||||
static void _failsafe_timer_event();
|
||||
static AP_HAL::TimedProc _failsafe;
|
||||
|
||||
static volatile bool _timer_suspended;
|
||||
static volatile bool _timer_event_missed;
|
||||
static AP_HAL::TimedProc _timer_proc[FLYMAPLE_SCHEDULER_MAX_TIMER_PROCS];
|
||||
static uint8_t _num_timer_procs;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_SCHEDULER_H__
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "Semaphores.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
#include "Semaphores.h"
|
||||
#include "Scheduler.h"
|
||||
#include "FlymapleWirish.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// Constructor
|
||||
FLYMAPLESemaphore::FLYMAPLESemaphore() : _taken(false) {}
|
||||
|
||||
bool FLYMAPLESemaphore::give() {
|
||||
if (!_taken) {
|
||||
return false;
|
||||
} else {
|
||||
_taken = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool FLYMAPLESemaphore::take(uint32_t timeout_ms) {
|
||||
if (hal.scheduler->in_timerprocess()) {
|
||||
hal.scheduler->panic(PSTR("PANIC: FLYMAPLESemaphore::take used from "
|
||||
"inside timer process"));
|
||||
return false; /* Never reached - panic does not return */
|
||||
}
|
||||
return _take_from_mainloop(timeout_ms);
|
||||
}
|
||||
|
||||
bool FLYMAPLESemaphore::take_nonblocking() {
|
||||
if (hal.scheduler->in_timerprocess()) {
|
||||
return _take_nonblocking();
|
||||
} else {
|
||||
return _take_from_mainloop(0);
|
||||
}
|
||||
}
|
||||
|
||||
bool FLYMAPLESemaphore::_take_from_mainloop(uint32_t timeout_ms) {
|
||||
/* Try to take immediately */
|
||||
if (_take_nonblocking()) {
|
||||
return true;
|
||||
} else if (timeout_ms == 0) {
|
||||
/* Return immediately if timeout is 0 */
|
||||
return false;
|
||||
}
|
||||
|
||||
do {
|
||||
/* Delay 1ms until we can successfully take, or we timed out */
|
||||
hal.scheduler->delay(1);
|
||||
timeout_ms--;
|
||||
if (_take_nonblocking()) {
|
||||
return true;
|
||||
}
|
||||
} while (timeout_ms > 0);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool FLYMAPLESemaphore::_take_nonblocking() {
|
||||
bool result = false;
|
||||
noInterrupts();
|
||||
if (!_taken) {
|
||||
_taken = true;
|
||||
result = true;
|
||||
}
|
||||
interrupts();
|
||||
return result;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_SEMAPHORE_H__
|
||||
#define __AP_HAL_FLYMAPLE_SEMAPHORE_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLESemaphore : public AP_HAL::Semaphore {
|
||||
public:
|
||||
FLYMAPLESemaphore();
|
||||
bool give();
|
||||
bool take(uint32_t timeout_ms);
|
||||
bool take_nonblocking();
|
||||
private:
|
||||
bool _take_from_mainloop(uint32_t timeout_ms);
|
||||
bool _take_nonblocking();
|
||||
|
||||
bool _taken;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_SEMAPHORE_H__
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include <string.h>
|
||||
#include "Storage.h"
|
||||
#include "FlymapleWirish.h"
|
||||
// EEPROM.*, flash_stm* copied from AeroQuad_v3.2
|
||||
#include "utility/EEPROM.h"
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
static EEPROMClass eeprom;
|
||||
|
||||
FLYMAPLEStorage::FLYMAPLEStorage()
|
||||
{}
|
||||
|
||||
void FLYMAPLEStorage::init(void*)
|
||||
{
|
||||
eeprom.init(0x801F000, 0x801F800, 0x800);
|
||||
}
|
||||
|
||||
uint8_t FLYMAPLEStorage::read_byte(uint16_t loc){
|
||||
// 'bytes' are packed 2 per word
|
||||
// Read existing dataword and change upper or lower byte
|
||||
uint16_t data = eeprom.read(loc >> 1);
|
||||
if (loc & 1)
|
||||
return data >> 8;
|
||||
else
|
||||
return data & 0xff;
|
||||
}
|
||||
|
||||
uint16_t FLYMAPLEStorage::read_word(uint16_t loc){
|
||||
return eeprom.read(loc);
|
||||
}
|
||||
|
||||
uint32_t FLYMAPLEStorage::read_dword(uint16_t loc){
|
||||
loc <<= 1;
|
||||
uint32_t data = eeprom.read(loc);
|
||||
data |= (eeprom.read(loc+1) << 16); // second word is the high word
|
||||
return data;
|
||||
}
|
||||
|
||||
void FLYMAPLEStorage::read_block(void* dst, uint16_t src, size_t n) {
|
||||
// Treat as a block of bytes
|
||||
for (size_t i = 0; i < n; i++)
|
||||
((uint8_t*)dst)[i] = read_byte(src+i);
|
||||
}
|
||||
|
||||
void FLYMAPLEStorage::write_byte(uint16_t loc, uint8_t value)
|
||||
{
|
||||
// 'bytes' are packed 2 per word
|
||||
// Read existing data word and change upper or lower byte
|
||||
uint16_t data = eeprom.read(loc >> 1);
|
||||
if (loc & 1)
|
||||
data = (data & 0x00ff) | (value << 8); // Odd, upper byte
|
||||
else
|
||||
data = (data & 0xff00) | value; // Even, lower byte
|
||||
eeprom.write(loc >> 1, data);
|
||||
}
|
||||
|
||||
void FLYMAPLEStorage::write_word(uint16_t loc, uint16_t value)
|
||||
{
|
||||
eeprom.write(loc, value);
|
||||
}
|
||||
|
||||
void FLYMAPLEStorage::write_dword(uint16_t loc, uint32_t value)
|
||||
{
|
||||
loc <<= 1;
|
||||
eeprom.write(loc, value & 0xffff);
|
||||
eeprom.write(loc+1, value >> 16);
|
||||
}
|
||||
|
||||
void FLYMAPLEStorage::write_block(uint16_t loc, const void* src, size_t n)
|
||||
{
|
||||
// Treat as a block of bytes
|
||||
for (size_t i = 0; i < n; i++)
|
||||
write_byte(loc+i, ((uint8_t*)src)[i]);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_STORAGE_H__
|
||||
#define __AP_HAL_FLYMAPLE_STORAGE_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEStorage : public AP_HAL::Storage {
|
||||
public:
|
||||
FLYMAPLEStorage();
|
||||
void init(void *);
|
||||
uint8_t read_byte(uint16_t loc);
|
||||
uint16_t read_word(uint16_t loc);
|
||||
uint32_t read_dword(uint16_t loc);
|
||||
void read_block(void *dst, uint16_t src, size_t n);
|
||||
|
||||
void write_byte(uint16_t loc, uint8_t value);
|
||||
void write_word(uint16_t loc, uint16_t value);
|
||||
void write_dword(uint16_t loc, uint32_t value);
|
||||
void write_block(uint16_t dst, const void* src, size_t n);
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_STORAGE_H__
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "UARTDriver.h"
|
||||
#include "FlymapleWirish.h"
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
FLYMAPLEUARTDriver::FLYMAPLEUARTDriver(HardwareSerial* hws):
|
||||
_hws(hws)
|
||||
{}
|
||||
|
||||
void FLYMAPLEUARTDriver::begin(uint32_t b)
|
||||
{
|
||||
_hws->begin(b);
|
||||
}
|
||||
|
||||
void FLYMAPLEUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
||||
{
|
||||
begin(b);
|
||||
}
|
||||
|
||||
void FLYMAPLEUARTDriver::end()
|
||||
{
|
||||
_hws->end();
|
||||
}
|
||||
|
||||
void FLYMAPLEUARTDriver::flush()
|
||||
{
|
||||
_hws->flush();
|
||||
}
|
||||
|
||||
bool FLYMAPLEUARTDriver::is_initialized()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
void FLYMAPLEUARTDriver::set_blocking_writes(bool blocking) {}
|
||||
|
||||
bool FLYMAPLEUARTDriver::tx_pending() { return false; }
|
||||
|
||||
/* FLYMAPLE implementations of Stream virtual methods */
|
||||
int16_t FLYMAPLEUARTDriver::available()
|
||||
{
|
||||
return _hws->available();
|
||||
}
|
||||
|
||||
int16_t FLYMAPLEUARTDriver::txspace()
|
||||
{
|
||||
// txspace must return something sensible, else MAVlink wont s4nd out heartbeats
|
||||
// but we cannot get the actual number from libmaple.
|
||||
// Actually libmaple usart is synchronous, so the number returned is irrelevant
|
||||
return 1000;
|
||||
}
|
||||
|
||||
int16_t FLYMAPLEUARTDriver::read()
|
||||
{
|
||||
return _hws->read();
|
||||
}
|
||||
|
||||
/* FLYMAPLE implementations of Print virtual methods */
|
||||
size_t FLYMAPLEUARTDriver::write(uint8_t c)
|
||||
{
|
||||
_hws->write(c);
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_UARTDRIVER_H__
|
||||
#define __AP_HAL_FLYMAPLE_UARTDRIVER_H__
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
class HardwareSerial; // A libmaple classs
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEUARTDriver : public AP_HAL::UARTDriver {
|
||||
public:
|
||||
FLYMAPLEUARTDriver(HardwareSerial* hws);
|
||||
|
||||
/* FLYMAPLE implementations of UARTDriver virtual methods */
|
||||
void begin(uint32_t b);
|
||||
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
|
||||
void end();
|
||||
void flush();
|
||||
bool is_initialized();
|
||||
void set_blocking_writes(bool blocking);
|
||||
bool tx_pending();
|
||||
|
||||
/* FLYMAPLE implementations of Stream virtual methods */
|
||||
int16_t available();
|
||||
int16_t txspace();
|
||||
int16_t read();
|
||||
|
||||
/* FLYMAPLE implementations of Print virtual methods */
|
||||
size_t write(uint8_t c);
|
||||
private:
|
||||
HardwareSerial* _hws;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_UARTDRIVER_H__
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
/*
|
||||
This program 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.
|
||||
|
||||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Flymaple port by Mike McCauley
|
||||
*/
|
||||
|
||||
#ifndef __AP_HAL_FLYMAPLE_UTIL_H__
|
||||
#define __AP_HAL_FLYMAPLE_UTIL_H__
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include "AP_HAL_FLYMAPLE_Namespace.h"
|
||||
|
||||
class AP_HAL_FLYMAPLE_NS::FLYMAPLEUtil : public AP_HAL::Util {
|
||||
public:
|
||||
bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_UTIL_H__
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
/*
|
||||
* Example of APM_BMP085 (absolute pressure sensor) library.
|
||||
* Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <AP_InertialSensor.h>
|
||||
#include <math.h>
|
||||
#include <AP_Progmem.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Buffer.h>
|
||||
#include <Filter.h>
|
||||
#include <AP_Baro.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
AP_Baro_BMP085 bmp085;
|
||||
|
||||
uint32_t timer;
|
||||
|
||||
void setup()
|
||||
{
|
||||
hal.console->println("ArduPilot Mega BMP085 library test");
|
||||
hal.console->println("Initialising barometer...");
|
||||
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
if (!bmp085.init()) {
|
||||
hal.console->println("Barometer initialisation FAILED\n");
|
||||
}
|
||||
hal.console->println("initialisation complete.");
|
||||
hal.scheduler->delay(1000);
|
||||
timer = hal.scheduler->micros();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
float tmp_float;
|
||||
static uint32_t last_print;
|
||||
|
||||
// accumulate values at 100Hz
|
||||
if ((hal.scheduler->micros()- timer) > 20000L) {
|
||||
bmp085.accumulate();
|
||||
timer = hal.scheduler->micros();
|
||||
}
|
||||
|
||||
// print at 10Hz
|
||||
if ((hal.scheduler->millis()- last_print) >= 100) {
|
||||
uint32_t start = hal.scheduler->micros();
|
||||
last_print = hal.scheduler->millis();
|
||||
bmp085.read();
|
||||
uint32_t read_time = hal.scheduler->micros() - start;
|
||||
if (! bmp085.healthy) {
|
||||
hal.console->println("not healthy");
|
||||
return;
|
||||
}
|
||||
hal.console->print("Pressure:");
|
||||
hal.console->print( bmp085.get_pressure());
|
||||
hal.console->print(" Temperature:");
|
||||
hal.console->print( bmp085.get_temperature());
|
||||
hal.console->print(" Altitude:");
|
||||
tmp_float = ( bmp085.get_pressure() / 101325.0);
|
||||
tmp_float = pow(tmp_float, 0.190295);
|
||||
float alt = 44330.0 * (1.0 - tmp_float);
|
||||
hal.console->print(alt);
|
||||
hal.console->printf(" t=%lu samples=%u",
|
||||
read_time,
|
||||
(unsigned)bmp085.get_pressure_samples());
|
||||
hal.console->println();
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
BOARD = mega
|
||||
include ../../../../mk/apm.mk
|
||||
|
||||
apm2:
|
||||
make -f Makefile EXTRAFLAGS="-DAPM2_HARDWARE=1"
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
||||
|
||||
//
|
||||
// Example code for the AP_HAL_FLYMAPLE AnalogIn
|
||||
//
|
||||
// This code is placed into the public domain.
|
||||
//
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
// Expects pin 15 to be connected to board VCC 3.3V
|
||||
static AP_HAL::AnalogSource *vcc_pin; // GPIO pin 15
|
||||
static AP_HAL::AnalogSource *batt_pin; // GPIO pin 20
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
hal.console->println("AnalogIn test starts");
|
||||
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
|
||||
batt_pin = hal.analogin->channel(20);
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
float vcc = vcc_pin->voltage_average();
|
||||
float batt = batt_pin->voltage_average();
|
||||
|
||||
// Flymaple board pin 20 is connected to the external battery supply
|
||||
// via a 24k/5.1k voltage divider. The schematic claims the divider is 25k/5k,
|
||||
// but the actual installed resistors are not so.
|
||||
batt *= 5.70588; // (24000+5100)/5100
|
||||
hal.console->printf("Vcc pin 15: %f\n", vcc);
|
||||
hal.console->printf("VIN (via pin 20): %f\n", batt);
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
AP_HAL::DigitalSource *a_led;
|
||||
AP_HAL::DigitalSource *b_led;
|
||||
AP_HAL::DigitalSource *c_led;
|
||||
|
||||
void loop (void) {
|
||||
hal.scheduler->delay(1000);
|
||||
hal.gpio->write(13, 1);
|
||||
|
||||
a_led->write(1);
|
||||
b_led->write(0);
|
||||
c_led->write(1);
|
||||
|
||||
hal.scheduler->delay(1000);
|
||||
hal.gpio->write(13, 0);
|
||||
|
||||
a_led->write(0);
|
||||
b_led->write(1);
|
||||
c_led->write(0);
|
||||
}
|
||||
|
||||
void setup (void) {
|
||||
hal.gpio->pinMode(13, GPIO_OUTPUT);
|
||||
hal.gpio->write(13, 0);
|
||||
|
||||
a_led = hal.gpio->channel(27);
|
||||
b_led = hal.gpio->channel(26);
|
||||
c_led = hal.gpio->channel(25);
|
||||
|
||||
a_led->mode(GPIO_OUTPUT);
|
||||
b_led->mode(GPIO_OUTPUT);
|
||||
c_led->mode(GPIO_OUTPUT);
|
||||
|
||||
a_led->write(0);
|
||||
b_led->write(0);
|
||||
c_led->write(0);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
||||
|
||||
//
|
||||
// Example code for the AP_HAL AVRUARTDriver, based on FastSerial
|
||||
//
|
||||
// This code is placed into the public domain.
|
||||
//
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
if (s->available() > 0) {
|
||||
int c;
|
||||
c = s->read();
|
||||
if (-1 != c) {
|
||||
s->write((uint8_t)c);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
|
||||
uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Read the uart, write to the console backend. */
|
||||
if (s->available() > 0) {
|
||||
int b;
|
||||
b = s->read();
|
||||
if (-1 != b) {
|
||||
uint8_t tmp[1];
|
||||
tmp[0] = (uint8_t) b;
|
||||
c->backend_write(tmp, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/* Loop back the console upon itself. */
|
||||
{
|
||||
int b;
|
||||
b = c->read();
|
||||
if (-1 != b) {
|
||||
c->write((uint8_t)b);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* Read the console backend, print to the uart. */
|
||||
{
|
||||
uint8_t tmp[1];
|
||||
int readback = c->backend_read(tmp, 1);
|
||||
if (readback > 0) {
|
||||
s->write(tmp[0]);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
// Test printing things
|
||||
//
|
||||
hal.scheduler->delay(5000); // Need time to connect to USB port
|
||||
hal.console->print("test");
|
||||
hal.console->println(" begin");
|
||||
hal.console->println(1000);
|
||||
hal.console->println(1000, 8);
|
||||
hal.console->println(1000, 10);
|
||||
hal.console->println(1000, 16);
|
||||
hal.console->println_P(PSTR("progmem"));
|
||||
hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
|
||||
#if 0
|
||||
// loopbacks make no sense with USB console on flymaple
|
||||
hal.console->println("loopback for 10sec:");
|
||||
stream_loopback(hal.console, 10000);
|
||||
hal.console->println("loopback done");
|
||||
|
||||
hal.console->println("opening backend:");
|
||||
|
||||
hal.console->backend_open();
|
||||
|
||||
const char hello[] = "hello world\r\n";
|
||||
hal.console->backend_write((const uint8_t*)hello, strlen(hello));
|
||||
|
||||
hal.console->println("loopback for 10sec:");
|
||||
stream_console_loopback(hal.console, hal.console, 10000);
|
||||
hal.console->println("loopback done");
|
||||
|
||||
hal.console->backend_close();
|
||||
hal.console->println("closed backend.");
|
||||
#endif
|
||||
hal.console->println("done.");
|
||||
for(;;);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void loop(void){}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*******************************************
|
||||
* Sample sketch that configures an HMC5883L 3 axis
|
||||
* magnetometer to continuous mode and reads back
|
||||
* the three axis of data.
|
||||
*******************************************/
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
#define HMC5883L 0x1E
|
||||
|
||||
void setup() {
|
||||
hal.console->printf_P(PSTR("Initializing HMC5883L at address %x\r\n"),
|
||||
HMC5883L);
|
||||
|
||||
uint8_t stat = hal.i2c->writeRegister(HMC5883L,0x02,0x00);
|
||||
if (stat == 0) {
|
||||
hal.console->printf_P(PSTR("successful init\r\n"));
|
||||
} else {
|
||||
hal.console->printf_P(PSTR("failed init: return status %d\r\n"),
|
||||
(int)stat);
|
||||
for(;;);
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
uint8_t data[6];
|
||||
//read 6 bytes (x,y,z) from the device
|
||||
uint8_t stat = hal.i2c->readRegisters(HMC5883L,0x03,6, data);
|
||||
|
||||
if (stat == 0){
|
||||
int x, y, z;
|
||||
x = data[0] << 8;
|
||||
x |= data[1];
|
||||
y = data[2] << 8;
|
||||
y |= data[3];
|
||||
z = data[4] << 8;
|
||||
z |= data[5];
|
||||
hal.console->printf_P(PSTR("x: %d y: %d z: %d \r\n"), x, y, z);
|
||||
} else {
|
||||
hal.console->printf_P(PSTR("i2c error: status %d\r\n"), (int)stat);
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void multiread(AP_HAL::RCInput* in, uint16_t* channels) {
|
||||
/* Multi-channel read method: */
|
||||
uint8_t valid;
|
||||
valid = in->read(channels, 8);
|
||||
hal.console->printf_P(
|
||||
PSTR("multi read %d: %d %d %d %d %d %d %d %d\r\n"),
|
||||
(int) valid,
|
||||
channels[0], channels[1], channels[2], channels[3],
|
||||
channels[4], channels[5], channels[6], channels[7]);
|
||||
}
|
||||
|
||||
void individualread(AP_HAL::RCInput* in, uint16_t* channels) {
|
||||
/* individual channel read method: */
|
||||
uint8_t valid;
|
||||
valid = in->valid_channels();
|
||||
for (int i = 0; i < 8; i++) {
|
||||
channels[i] = in->read(i);
|
||||
}
|
||||
hal.console->printf_P(
|
||||
PSTR("individual read %d: %d %d %d %d %d %d %d %d\r\n"),
|
||||
(int) valid,
|
||||
channels[0], channels[1], channels[2], channels[3],
|
||||
channels[4], channels[5], channels[6], channels[7]);
|
||||
}
|
||||
|
||||
|
||||
void multiwrite(AP_HAL::RCOutput* out, uint16_t* channels) {
|
||||
out->write(0, channels, 8);
|
||||
/* Upper channels duplicate lower channels*/
|
||||
out->write(8, channels, 8);
|
||||
}
|
||||
|
||||
void individualwrite(AP_HAL::RCOutput* out, uint16_t* channels) {
|
||||
for (int ch = 0; ch < 8; ch++) {
|
||||
out->write(ch, channels[ch]);
|
||||
/* Upper channels duplicate lower channels*/
|
||||
out->write(ch+8, channels[ch]);
|
||||
}
|
||||
}
|
||||
|
||||
void loop (void) {
|
||||
static int ctr = 0;
|
||||
uint16_t channels[8];
|
||||
|
||||
// hal.gpio->write(13, 1);
|
||||
|
||||
/* Cycle between using the individual read method
|
||||
* and the multi read method*/
|
||||
if (ctr < 500) {
|
||||
multiread(hal.rcin, channels);
|
||||
} else {
|
||||
individualread(hal.rcin, channels);
|
||||
if (ctr > 1000) ctr = 0;
|
||||
}
|
||||
|
||||
/* Cycle between individual output and multichannel output */
|
||||
if (ctr % 500 < 250) {
|
||||
multiwrite(hal.rcout, channels);
|
||||
} else {
|
||||
individualwrite(hal.rcout, channels);
|
||||
}
|
||||
|
||||
// hal.gpio->write(13, 0);
|
||||
hal.scheduler->delay(4);
|
||||
ctr++;
|
||||
}
|
||||
|
||||
void setup (void) {
|
||||
// hal.scheduler->delay(5000);
|
||||
hal.gpio->pinMode(13, GPIO_OUTPUT);
|
||||
hal.gpio->write(13, 0);
|
||||
hal.rcout->enable_mask(0x000000FF);
|
||||
|
||||
/* Bottom 4 channels at 400hz (like on a quad) */
|
||||
hal.rcout->set_freq(0x0000000F, 400);
|
||||
for(int i = 0; i < 12; i++) {
|
||||
hal.console->printf_P(PSTR("rcout ch %d has frequency %d\r\n"),
|
||||
i, hal.rcout->get_freq(i));
|
||||
}
|
||||
/* Delay to let the user see the above printouts on the terminal */
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
// Loopback test for SPI driver
|
||||
// Connect MISO and MOSI pins together (12 and 13 on Flymaple)
|
||||
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
AP_HAL::SPIDeviceDriver* spidev;
|
||||
|
||||
void setup (void) {
|
||||
hal.scheduler->delay(5000);
|
||||
hal.console->printf_P(PSTR("Starting AP_HAL_FLYMAPLE::SPIDriver test\r\n"));
|
||||
|
||||
spidev = hal.spi->device(AP_HAL::SPIDevice_MPU6000); // Not really MPU6000, just a generic SPU driver
|
||||
if (!spidev)
|
||||
hal.scheduler->panic(PSTR("Starting AP_HAL_FLYMAPLE::SPIDriver failed to get spidev\r\n"));
|
||||
}
|
||||
|
||||
void loop (void) {
|
||||
uint8_t tx_data[] = { 'h', 'e', 'l', 'l', 'o', 0 };
|
||||
uint8_t rx_data[] = { 0, 0, 0, 0, 0, 0 };
|
||||
|
||||
spidev->transaction(tx_data, rx_data, sizeof(tx_data));
|
||||
if (memcmp(tx_data, rx_data, sizeof(tx_data)))
|
||||
hal.console->println("Incorrect data read from SPI loopback. Do you have the loopback wire installed between pins 11 and 12?");
|
||||
else
|
||||
hal.console->println("Correct data read from SPI loopback");
|
||||
hal.scheduler->delay(500);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,115 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
/**
|
||||
* You'll want to use a logic analyzer to watch the effects of this test.
|
||||
* On the APM2 its pretty easy to hook up an analyzer to pins A0 through A3.
|
||||
*/
|
||||
#define DELAY_TOGGLE_PIN 15 /* A0 = pin 15 */
|
||||
#define FAILSAFE_TOGGLE_PIN 16 /* A1 = pin 16 */
|
||||
#define SCHEDULED_TOGGLE_PIN_1 17 /* A2 = pin 17 */
|
||||
#define SCHEDULED_TOGGLE_PIN_2 18 /* A3 = pin 18 */
|
||||
|
||||
void delay_toggle() {
|
||||
volatile int i;
|
||||
hal.gpio->write(DELAY_TOGGLE_PIN, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(DELAY_TOGGLE_PIN, 0);
|
||||
}
|
||||
|
||||
void failsafe_toggle(uint32_t machtnichts) {
|
||||
volatile int i;
|
||||
hal.gpio->write(FAILSAFE_TOGGLE_PIN, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(FAILSAFE_TOGGLE_PIN, 0);
|
||||
}
|
||||
|
||||
|
||||
void schedule_toggle_1(uint32_t machtnichts) {
|
||||
volatile int i;
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_1, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_1, 0);
|
||||
}
|
||||
|
||||
void schedule_toggle_2(uint32_t machtnichts) {
|
||||
volatile int i;
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 0);
|
||||
}
|
||||
|
||||
void schedule_toggle_hang(uint32_t machtnichts) {
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 1);
|
||||
for(;;);
|
||||
}
|
||||
|
||||
void setup_pin(int pin_num) {
|
||||
hal.console->printf_P(PSTR("Setup pin %d\r\n"), pin_num);
|
||||
hal.gpio->pinMode(pin_num,GPIO_OUTPUT);
|
||||
/* Blink so we can see setup on the logic analyzer.*/
|
||||
hal.gpio->write(pin_num,1);
|
||||
hal.gpio->write(pin_num,0);
|
||||
}
|
||||
|
||||
void setup (void) {
|
||||
// hal.scheduler->delay(5000);
|
||||
hal.console->printf_P(PSTR("Starting AP_HAL_AVR::Scheduler test\r\n"));
|
||||
|
||||
setup_pin(DELAY_TOGGLE_PIN);
|
||||
setup_pin(FAILSAFE_TOGGLE_PIN);
|
||||
setup_pin(SCHEDULED_TOGGLE_PIN_1);
|
||||
setup_pin(SCHEDULED_TOGGLE_PIN_2);
|
||||
|
||||
hal.console->printf_P(PSTR("Testing delay callback. "
|
||||
"Pin %d should toggle at 1khz:\r\n"),
|
||||
(int) DELAY_TOGGLE_PIN);
|
||||
|
||||
// hal.scheduler->register_delay_callback(delay_toggle,0);
|
||||
|
||||
hal.scheduler->delay(2000);
|
||||
hal.console->printf_P(PSTR("Testing failsafe callback. "
|
||||
"Pin %d should toggle at 1khz:\r\n"),
|
||||
(int) FAILSAFE_TOGGLE_PIN);
|
||||
|
||||
hal.scheduler->register_timer_failsafe(failsafe_toggle, 1000);
|
||||
|
||||
hal.scheduler->delay(2000);
|
||||
|
||||
hal.console->printf_P(PSTR("Testing running timer processes.\r\n"));
|
||||
hal.console->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
|
||||
(int) SCHEDULED_TOGGLE_PIN_1);
|
||||
hal.console->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
|
||||
(int) SCHEDULED_TOGGLE_PIN_2);
|
||||
|
||||
hal.scheduler->register_timer_process(schedule_toggle_1);
|
||||
hal.scheduler->register_timer_process(schedule_toggle_2);
|
||||
|
||||
hal.scheduler->delay(2000);
|
||||
|
||||
// not yet working on flymaple: see FLYMAPLEScheduler::_timer_procs_timer_event()
|
||||
#if 1
|
||||
hal.console->printf_P(PSTR("Test running a pathological timer process.\r\n"
|
||||
"Failsafe should continue even as pathological process "
|
||||
"dominates the processor."));
|
||||
hal.console->printf_P(PSTR("Pin %d should toggle then go high forever.\r\n"),
|
||||
(int) SCHEDULED_TOGGLE_PIN_2);
|
||||
hal.scheduler->register_timer_process(schedule_toggle_hang);
|
||||
#endif
|
||||
|
||||
// Wait a little then test reboot
|
||||
// hal.scheduler->delay(5000);
|
||||
// hal.scheduler->reboot();
|
||||
}
|
||||
|
||||
void loop (void) { }
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
/**
|
||||
* You'll want to use a logic analyzer to watch the effects of this test.
|
||||
* On the APM2 its pretty easy to hook up an analyzer to pins A0 through A3.
|
||||
*/
|
||||
#define PIN_A0 15 /* A0 */
|
||||
#define PIN_A1 16 /* A1 */
|
||||
#define PIN_A2 17 /* A2 */
|
||||
#define PIN_A3 18 /* A3 */
|
||||
|
||||
/**
|
||||
* Create an AVRSemaphore for this test.
|
||||
*/
|
||||
|
||||
AP_HAL::Semaphore *sem;
|
||||
|
||||
void blink_a0() {
|
||||
volatile int i;
|
||||
hal.gpio->write(PIN_A0, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(PIN_A0, 0);
|
||||
}
|
||||
|
||||
void blink_a1() {
|
||||
volatile int i;
|
||||
hal.gpio->write(PIN_A1, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(PIN_A1, 0);
|
||||
}
|
||||
|
||||
|
||||
void blink_a2() {
|
||||
volatile int i;
|
||||
hal.gpio->write(PIN_A2, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(PIN_A2, 0);
|
||||
}
|
||||
|
||||
void blink_a3() {
|
||||
volatile int i;
|
||||
hal.gpio->write(PIN_A3, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
hal.gpio->write(PIN_A3, 0);
|
||||
}
|
||||
|
||||
void setup_pin(int pin_num) {
|
||||
hal.console->printf_P(PSTR("Setup pin %d\r\n"), pin_num);
|
||||
hal.gpio->pinMode(pin_num,GPIO_OUTPUT);
|
||||
/* Blink so we can see setup on the logic analyzer.*/
|
||||
hal.gpio->write(pin_num,1);
|
||||
hal.gpio->write(pin_num,0);
|
||||
}
|
||||
|
||||
void setup (void) {
|
||||
hal.console->printf_P(PSTR("Starting Semaphore test\r\n"));
|
||||
|
||||
setup_pin(PIN_A0);
|
||||
setup_pin(PIN_A1);
|
||||
setup_pin(PIN_A2);
|
||||
setup_pin(PIN_A3);
|
||||
|
||||
hal.console->printf_P(PSTR("Using SPIDeviceManager builtin Semaphore\r\n"));
|
||||
|
||||
AP_HAL::SPIDeviceDriver *dataflash = hal.spi->device(AP_HAL::SPIDevice_Dataflash); // not really
|
||||
|
||||
if (dataflash == NULL) {
|
||||
hal.scheduler->panic(PSTR("Error: No SPIDeviceDriver!"));
|
||||
}
|
||||
|
||||
sem = dataflash->get_semaphore();
|
||||
if (sem == NULL) {
|
||||
hal.scheduler->panic(PSTR("Error: No SPIDeviceDriver semaphore!"));
|
||||
}
|
||||
|
||||
hal.scheduler->register_timer_process(async_blinker);
|
||||
}
|
||||
|
||||
|
||||
uint32_t async_last_run = 0;
|
||||
void async_blinker(uint32_t millis) {
|
||||
if (async_last_run - millis < 5) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (sem->take_nonblocking()) {
|
||||
blink_a0();
|
||||
sem->give();
|
||||
} else {
|
||||
blink_a1();
|
||||
}
|
||||
}
|
||||
|
||||
void loop (void) {
|
||||
|
||||
if (sem->take(1)) {
|
||||
blink_a2();
|
||||
sem->give();
|
||||
} else {
|
||||
/* This should never happen: */
|
||||
blink_a3();
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
uint8_t fibs[] = { 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144 };
|
||||
|
||||
void test_erase() {
|
||||
hal.console->printf_P(PSTR("erasing... "));
|
||||
for(int i = 0; i < 100; i++) {
|
||||
hal.storage->write_byte(i, 0);
|
||||
}
|
||||
hal.console->printf_P(PSTR(" done.\r\n"));
|
||||
}
|
||||
|
||||
void test_write() {
|
||||
hal.console->printf_P(PSTR("writing... "));
|
||||
hal.storage->write_block(0, fibs, sizeof(fibs));
|
||||
hal.console->printf_P(PSTR(" done.\r\n"));
|
||||
}
|
||||
|
||||
void test_readback() {
|
||||
hal.console->printf_P(PSTR("reading back...\r\n"));
|
||||
uint8_t readback[sizeof(fibs)];
|
||||
bool success = true;
|
||||
hal.storage->read_block(readback, 0, sizeof(fibs));
|
||||
for (int i = 0; i < sizeof(fibs); i++) {
|
||||
if (readback[i] != fibs[i]) {
|
||||
success = false;
|
||||
hal.console->printf_P(PSTR("At index %d expected %d got %d\r\n"),
|
||||
i, (int) fibs[i], (int) readback[i]);
|
||||
}
|
||||
}
|
||||
if (success) {
|
||||
hal.console->printf_P(PSTR("all bytes read successfully\r\n"));
|
||||
}
|
||||
hal.console->printf_P(PSTR("done reading back.\r\n"));
|
||||
}
|
||||
|
||||
void setup (void) {
|
||||
hal.scheduler->delay(5000);
|
||||
hal.console->printf_P(PSTR("Starting AP_HAL_FLYMAPLE::Storage test\r\n"));
|
||||
test_erase();
|
||||
test_write();
|
||||
test_readback();
|
||||
}
|
||||
|
||||
void loop (void) { }
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
||||
|
||||
//
|
||||
// Example code for the AP_HAL AVRUARTDriver, based on FastSerial
|
||||
//
|
||||
// This code is placed into the public domain.
|
||||
//
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
hal.uartA->begin(115200);
|
||||
|
||||
//
|
||||
// Test printing things
|
||||
//
|
||||
|
||||
hal.uartA->print("test");
|
||||
hal.uartA->println(" begin");
|
||||
hal.uartA->println(1000);
|
||||
hal.uartA->println(1000, 8);
|
||||
hal.uartA->println(1000, 10);
|
||||
hal.uartA->println(1000, 16);
|
||||
hal.uartA->println_P(PSTR("progmem"));
|
||||
int x = 3;
|
||||
int *ptr = &x;
|
||||
hal.uartA->printf("printf %d %u %#x %p %f %s\n", -1000, 1000, 1000, ptr, 1.2345, PSTR("progmem"));
|
||||
hal.uartA->printf_P(PSTR("printf_P %d %u %#x %p %f %s\n"), -1000, 1000, 1000, ptr, 1.2345, PSTR("progmem"));
|
||||
hal.uartA->println("done");
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
int c;
|
||||
//
|
||||
// Perform a simple loopback operation.
|
||||
//
|
||||
c = hal.uartA->read();
|
||||
if (-1 != c)
|
||||
hal.uartA->write(c);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Progmem.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void test_snprintf_P() {
|
||||
char test[40];
|
||||
memset(test,0,40);
|
||||
hal.util->snprintf_P(test, 40, PSTR("hello %d from prog %f %S\r\n"),
|
||||
10, 1.2345, PSTR("progmem"));
|
||||
hal.console->write((const uint8_t*)test, strlen(test));
|
||||
|
||||
}
|
||||
|
||||
void test_snprintf() {
|
||||
char test[40];
|
||||
memset(test,0,40);
|
||||
hal.util->snprintf(test, 40, "hello %d world %f %s\r\n",
|
||||
20, 2.3456, "sarg");
|
||||
hal.console->write((const uint8_t*)test, strlen(test));
|
||||
}
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
// HAL will start serial port at 115200.
|
||||
//
|
||||
|
||||
//
|
||||
// Test printing things
|
||||
//
|
||||
hal.console->println("Utility String Library Test");
|
||||
hal.console->println("Test snprintf:");
|
||||
|
||||
test_snprintf();
|
||||
|
||||
hal.console->println("Test snprintf_P:");
|
||||
|
||||
test_snprintf_P();
|
||||
|
||||
hal.console->println("done");
|
||||
}
|
||||
|
||||
void loop(void) { }
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Progmem.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Math.h>
|
||||
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void setup() {
|
||||
hal.scheduler->delay(5000);
|
||||
hal.console->println_P(PSTR("Empty setup"));
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
hal.console->println_P(PSTR("loop"));
|
||||
hal.console->printf("hello %d\n", 1234);
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
|
|
@ -0,0 +1,553 @@
|
|||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "wirish.h"
|
||||
#include "EEPROM.h"
|
||||
|
||||
/**
|
||||
* @brief Check page for blank
|
||||
* @param page base address
|
||||
* @retval Success or error
|
||||
* EEPROM_BAD_FLASH: page not empty after erase
|
||||
* EEPROM_OK: page blank
|
||||
*/
|
||||
uint16 EEPROMClass::EE_CheckPage(uint32 pageBase, uint16 status)
|
||||
{
|
||||
uint32 pageEnd = pageBase + (uint32)PageSize;
|
||||
|
||||
// Page Status not EEPROM_ERASED and not a "state"
|
||||
if ((*(__io uint16*)pageBase) != EEPROM_ERASED && (*(__io uint16*)pageBase) != status)
|
||||
return EEPROM_BAD_FLASH;
|
||||
for(pageBase += 4; pageBase < pageEnd; pageBase += 4)
|
||||
if ((*(__io uint32*)pageBase) != 0xFFFFFFFF) // Verify if slot is empty
|
||||
return EEPROM_BAD_FLASH;
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Erase page with increment erase counter (page + 2)
|
||||
* @param page base address
|
||||
* @retval Success or error
|
||||
* FLASH_COMPLETE: success erase
|
||||
* - Flash error code: on write Flash error
|
||||
*/
|
||||
FLASH_Status EEPROMClass::EE_ErasePage(uint32 pageBase)
|
||||
{
|
||||
FLASH_Status FlashStatus;
|
||||
uint16 data = (*(__io uint16*)(pageBase));
|
||||
if ((data == EEPROM_ERASED) || (data == EEPROM_VALID_PAGE) || (data == EEPROM_RECEIVE_DATA))
|
||||
data = (*(__io uint16*)(pageBase + 2)) + 1;
|
||||
else
|
||||
data = 0;
|
||||
|
||||
FlashStatus = FLASH_ErasePage(pageBase);
|
||||
if (FlashStatus == FLASH_COMPLETE)
|
||||
FlashStatus = FLASH_ProgramHalfWord(pageBase + 2, data);
|
||||
|
||||
return FlashStatus;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check page for blank and erase it
|
||||
* @param page base address
|
||||
* @retval Success or error
|
||||
* - Flash error code: on write Flash error
|
||||
* - EEPROM_BAD_FLASH: page not empty after erase
|
||||
* - EEPROM_OK: page blank
|
||||
*/
|
||||
uint16 EEPROMClass::EE_CheckErasePage(uint32 pageBase, uint16 status)
|
||||
{
|
||||
uint16 FlashStatus;
|
||||
if (EE_CheckPage(pageBase, status) != EEPROM_OK)
|
||||
{
|
||||
FlashStatus = EE_ErasePage(pageBase);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
return EE_CheckPage(pageBase, status);
|
||||
}
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Find valid Page for write or read operation
|
||||
* @param Page0: Page0 base address
|
||||
* Page1: Page1 base address
|
||||
* @retval Valid page address (PAGE0 or PAGE1) or NULL in case of no valid page was found
|
||||
*/
|
||||
uint32 EEPROMClass::EE_FindValidPage(void)
|
||||
{
|
||||
uint16 status0 = (*(__io uint16*)PageBase0); // Get Page0 actual status
|
||||
uint16 status1 = (*(__io uint16*)PageBase1); // Get Page1 actual status
|
||||
|
||||
if (status0 == EEPROM_VALID_PAGE && status1 == EEPROM_ERASED)
|
||||
return PageBase0;
|
||||
if (status1 == EEPROM_VALID_PAGE && status0 == EEPROM_ERASED)
|
||||
return PageBase1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculate unique variables in EEPROM
|
||||
* @param start: address of first slot to check (page + 4)
|
||||
* @param end: page end address
|
||||
* @param address: 16 bit virtual address of the variable to excluse (or 0XFFFF)
|
||||
* @retval count of variables
|
||||
*/
|
||||
uint16 EEPROMClass::EE_GetVariablesCount(uint32 pageBase, uint16 skipAddress)
|
||||
{
|
||||
uint16 varAddress, nextAddress;
|
||||
uint32 idx;
|
||||
uint32 pageEnd = pageBase + (uint32)PageSize;
|
||||
uint16 mycount = 0;
|
||||
|
||||
for (pageBase += 6; pageBase < pageEnd; pageBase += 4)
|
||||
{
|
||||
varAddress = (*(__io uint16*)pageBase);
|
||||
if (varAddress == 0xFFFF || varAddress == skipAddress)
|
||||
continue;
|
||||
|
||||
mycount++;
|
||||
for(idx = pageBase + 4; idx < pageEnd; idx += 4)
|
||||
{
|
||||
nextAddress = (*(__io uint16*)idx);
|
||||
if (nextAddress == varAddress)
|
||||
{
|
||||
mycount--;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return mycount;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Transfers last updated variables data from the full Page to an empty one.
|
||||
* @param newPage: new page base address
|
||||
* @param oldPage: old page base address
|
||||
* @param SkipAddress: 16 bit virtual address of the variable (or 0xFFFF)
|
||||
* @retval Success or error status:
|
||||
* - FLASH_COMPLETE: on success
|
||||
* - EEPROM_OUT_SIZE: if valid new page is full
|
||||
* - Flash error code: on write Flash error
|
||||
*/
|
||||
uint16 EEPROMClass::EE_PageTransfer(uint32 newPage, uint32 oldPage, uint16 SkipAddress)
|
||||
{
|
||||
uint32 oldEnd, newEnd;
|
||||
uint32 oldIdx, newIdx, idx;
|
||||
uint16 address, data, found;
|
||||
FLASH_Status FlashStatus;
|
||||
|
||||
// Transfer process: transfer variables from old to the new active page
|
||||
newEnd = newPage + ((uint32)PageSize);
|
||||
|
||||
// Find first free element in new page
|
||||
for (newIdx = newPage + 4; newIdx < newEnd; newIdx += 4)
|
||||
if ((*(__io uint32*)newIdx) == 0xFFFFFFFF) // Verify if element
|
||||
break; // contents are 0xFFFFFFFF
|
||||
if (newIdx >= newEnd)
|
||||
return EEPROM_OUT_SIZE;
|
||||
|
||||
oldEnd = oldPage + 4;
|
||||
oldIdx = oldPage + (uint32)(PageSize - 2);
|
||||
|
||||
for (; oldIdx > oldEnd; oldIdx -= 4)
|
||||
{
|
||||
address = *(__io uint16*)oldIdx;
|
||||
if (address == 0xFFFF || address == SkipAddress)
|
||||
continue; // it's means that power off after write data
|
||||
|
||||
found = 0;
|
||||
for (idx = newPage + 6; idx < newIdx; idx += 4)
|
||||
if ((*(__io uint16*)(idx)) == address)
|
||||
{
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
if (found)
|
||||
continue;
|
||||
|
||||
if (newIdx < newEnd)
|
||||
{
|
||||
data = (*(__io uint16*)(oldIdx - 2));
|
||||
|
||||
FlashStatus = FLASH_ProgramHalfWord(newIdx, data);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
FlashStatus = FLASH_ProgramHalfWord(newIdx + 2, address);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
newIdx += 4;
|
||||
}
|
||||
else
|
||||
return EEPROM_OUT_SIZE;
|
||||
}
|
||||
|
||||
// Erase the old Page: Set old Page status to EEPROM_EEPROM_ERASED status
|
||||
data = EE_CheckErasePage(oldPage, EEPROM_ERASED);
|
||||
if (data != EEPROM_OK)
|
||||
return data;
|
||||
|
||||
// Set new Page status
|
||||
FlashStatus = FLASH_ProgramHalfWord(newPage, EEPROM_VALID_PAGE);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Verify if active page is full and Writes variable in EEPROM.
|
||||
* @param Address: 16 bit virtual address of the variable
|
||||
* @param Data: 16 bit data to be written as variable value
|
||||
* @retval Success or error status:
|
||||
* - FLASH_COMPLETE: on success
|
||||
* - EEPROM_PAGE_FULL: if valid page is full (need page transfer)
|
||||
* - EEPROM_NO_VALID_PAGE: if no valid page was found
|
||||
* - EEPROM_OUT_SIZE: if EEPROM size exceeded
|
||||
* - Flash error code: on write Flash error
|
||||
*/
|
||||
uint16 EEPROMClass::EE_VerifyPageFullWriteVariable(uint16 Address, uint16 Data)
|
||||
{
|
||||
FLASH_Status FlashStatus;
|
||||
uint32 idx, pageBase, pageEnd, newPage;
|
||||
uint16 mycount;
|
||||
|
||||
// Get valid Page for write operation
|
||||
pageBase = EE_FindValidPage();
|
||||
if (pageBase == 0)
|
||||
return EEPROM_NO_VALID_PAGE;
|
||||
|
||||
// Get the valid Page end Address
|
||||
pageEnd = pageBase + PageSize; // Set end of page
|
||||
|
||||
for (idx = pageEnd - 2; idx > pageBase; idx -= 4)
|
||||
{
|
||||
if ((*(__io uint16*)idx) == Address) // Find last value for address
|
||||
{
|
||||
mycount = (*(__io uint16*)(idx - 2)); // Read last data
|
||||
if (mycount == Data)
|
||||
return EEPROM_OK;
|
||||
if (mycount == 0xFFFF)
|
||||
{
|
||||
FlashStatus = FLASH_ProgramHalfWord(idx - 2, Data); // Set variable data
|
||||
if (FlashStatus == FLASH_COMPLETE)
|
||||
return EEPROM_OK;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Check each active page address starting from begining
|
||||
for (idx = pageBase + 4; idx < pageEnd; idx += 4)
|
||||
if ((*(__io uint32*)idx) == 0xFFFFFFFF) // Verify if element
|
||||
{ // contents are 0xFFFFFFFF
|
||||
FlashStatus = FLASH_ProgramHalfWord(idx, Data); // Set variable data
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
FlashStatus = FLASH_ProgramHalfWord(idx + 2, Address); // Set variable virtual address
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
// Empty slot not found, need page transfer
|
||||
// Calculate unique variables in page
|
||||
mycount = EE_GetVariablesCount(pageBase, Address) + 1;
|
||||
if (mycount >= (PageSize / 4 - 1))
|
||||
return EEPROM_OUT_SIZE;
|
||||
|
||||
if (pageBase == PageBase1)
|
||||
newPage = PageBase0; // New page address where variable will be moved to
|
||||
else
|
||||
newPage = PageBase1;
|
||||
|
||||
// Set the new Page status to RECEIVE_DATA status
|
||||
FlashStatus = FLASH_ProgramHalfWord(newPage, EEPROM_RECEIVE_DATA);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
// Write the variable passed as parameter in the new active page
|
||||
FlashStatus = FLASH_ProgramHalfWord(newPage + 4, Data);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
FlashStatus = FLASH_ProgramHalfWord(newPage + 6, Address);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
|
||||
return EE_PageTransfer(newPage, pageBase, Address);
|
||||
}
|
||||
|
||||
EEPROMClass::EEPROMClass(void)
|
||||
{
|
||||
PageBase0 = EEPROM_PAGE0_BASE;
|
||||
PageBase1 = EEPROM_PAGE1_BASE;
|
||||
PageSize = EEPROM_PAGE_SIZE;
|
||||
Status = EEPROM_NOT_INIT;
|
||||
}
|
||||
|
||||
uint16 EEPROMClass::init(uint32 pageBase0, uint32 pageBase1, uint32 pageSize)
|
||||
{
|
||||
PageBase0 = pageBase0;
|
||||
PageBase1 = pageBase1;
|
||||
PageSize = pageSize;
|
||||
return init();
|
||||
}
|
||||
|
||||
uint16 EEPROMClass::init(void)
|
||||
{
|
||||
uint16 status0, status1;
|
||||
FLASH_Status FlashStatus;
|
||||
|
||||
FLASH_Unlock();
|
||||
Status = EEPROM_NO_VALID_PAGE;
|
||||
|
||||
status0 = (*(__io uint16 *)PageBase0);
|
||||
status1 = (*(__io uint16 *)PageBase1);
|
||||
|
||||
switch (status0)
|
||||
{
|
||||
/*
|
||||
Page0 Page1
|
||||
----- -----
|
||||
EEPROM_ERASED EEPROM_VALID_PAGE Page1 valid, Page0 erased
|
||||
EEPROM_RECEIVE_DATA Page1 need set to valid, Page0 erased
|
||||
EEPROM_ERASED make EE_Format
|
||||
any Error: EEPROM_NO_VALID_PAGE
|
||||
*/
|
||||
case EEPROM_ERASED:
|
||||
if (status1 == EEPROM_VALID_PAGE) // Page0 erased, Page1 valid
|
||||
Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
|
||||
else if (status1 == EEPROM_RECEIVE_DATA) // Page0 erased, Page1 receive
|
||||
{
|
||||
FlashStatus = FLASH_ProgramHalfWord(PageBase1, EEPROM_VALID_PAGE);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
Status = FlashStatus;
|
||||
else
|
||||
Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
|
||||
}
|
||||
else if (status1 == EEPROM_ERASED) // Both in erased state so format EEPROM
|
||||
Status = format();
|
||||
break;
|
||||
/*
|
||||
Page0 Page1
|
||||
----- -----
|
||||
EEPROM_RECEIVE_DATA EEPROM_VALID_PAGE Transfer Page1 to Page0
|
||||
EEPROM_ERASED Page0 need set to valid, Page1 erased
|
||||
any EEPROM_NO_VALID_PAGE
|
||||
*/
|
||||
case EEPROM_RECEIVE_DATA:
|
||||
if (status1 == EEPROM_VALID_PAGE) // Page0 receive, Page1 valid
|
||||
Status = EE_PageTransfer(PageBase0, PageBase1, 0xFFFF);
|
||||
else if (status1 == EEPROM_ERASED) // Page0 receive, Page1 erased
|
||||
{
|
||||
Status = EE_CheckErasePage(PageBase1, EEPROM_ERASED);
|
||||
if (Status == EEPROM_OK)
|
||||
{
|
||||
FlashStatus = FLASH_ProgramHalfWord(PageBase0, EEPROM_VALID_PAGE);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
Status = FlashStatus;
|
||||
else
|
||||
Status = EEPROM_OK;
|
||||
}
|
||||
}
|
||||
break;
|
||||
/*
|
||||
Page0 Page1
|
||||
----- -----
|
||||
EEPROM_VALID_PAGE EEPROM_VALID_PAGE Error: EEPROM_NO_VALID_PAGE
|
||||
EEPROM_RECEIVE_DATA Transfer Page0 to Page1
|
||||
any Page0 valid, Page1 erased
|
||||
*/
|
||||
case EEPROM_VALID_PAGE:
|
||||
if (status1 == EEPROM_VALID_PAGE) // Both pages valid
|
||||
Status = EEPROM_NO_VALID_PAGE;
|
||||
else if (status1 == EEPROM_RECEIVE_DATA)
|
||||
Status = EE_PageTransfer(PageBase1, PageBase0, 0xFFFF);
|
||||
else
|
||||
Status = EE_CheckErasePage(PageBase1, EEPROM_ERASED);
|
||||
break;
|
||||
/*
|
||||
Page0 Page1
|
||||
----- -----
|
||||
any EEPROM_VALID_PAGE Page1 valid, Page0 erased
|
||||
EEPROM_RECEIVE_DATA Page1 valid, Page0 erased
|
||||
any EEPROM_NO_VALID_PAGE
|
||||
*/
|
||||
default:
|
||||
if (status1 == EEPROM_VALID_PAGE)
|
||||
Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED); // Check/Erase Page0
|
||||
else if (status1 == EEPROM_RECEIVE_DATA)
|
||||
{
|
||||
FlashStatus = FLASH_ProgramHalfWord(PageBase1, EEPROM_VALID_PAGE);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
Status = FlashStatus;
|
||||
else
|
||||
Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
|
||||
}
|
||||
break;
|
||||
}
|
||||
return Status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Erases PAGE0 and PAGE1 and writes EEPROM_VALID_PAGE / 0 header to PAGE0
|
||||
* @param PAGE0 and PAGE1 base addresses
|
||||
* @retval Status of the last operation (Flash write or erase) done during EEPROM formating
|
||||
*/
|
||||
uint16 EEPROMClass::format(void)
|
||||
{
|
||||
uint16 status;
|
||||
FLASH_Status FlashStatus;
|
||||
|
||||
FLASH_Unlock();
|
||||
|
||||
// Erase Page0
|
||||
status = EE_CheckErasePage(PageBase0, EEPROM_VALID_PAGE);
|
||||
if (status != EEPROM_OK)
|
||||
return status;
|
||||
if ((*(__io uint16*)PageBase0) == EEPROM_ERASED)
|
||||
{
|
||||
// Set Page0 as valid page: Write VALID_PAGE at Page0 base address
|
||||
FlashStatus = FLASH_ProgramHalfWord(PageBase0, EEPROM_VALID_PAGE);
|
||||
if (FlashStatus != FLASH_COMPLETE)
|
||||
return FlashStatus;
|
||||
}
|
||||
// Erase Page1
|
||||
return EE_CheckErasePage(PageBase1, EEPROM_ERASED);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the erase counter for current page
|
||||
* @param Data: Global variable contains the read variable value
|
||||
* @retval Success or error status:
|
||||
* - EEPROM_OK: if erases counter return.
|
||||
* - EEPROM_NO_VALID_PAGE: if no valid page was found.
|
||||
*/
|
||||
uint16 EEPROMClass::erases(uint16 *Erases)
|
||||
{
|
||||
uint32 pageBase;
|
||||
if (Status != EEPROM_OK)
|
||||
if (init() != EEPROM_OK)
|
||||
return Status;
|
||||
|
||||
// Get active Page for read operation
|
||||
pageBase = EE_FindValidPage();
|
||||
if (pageBase == 0)
|
||||
return EEPROM_NO_VALID_PAGE;
|
||||
|
||||
*Erases = (*(__io uint16*)pageBase+2);
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the last stored variable data, if found,
|
||||
* which correspond to the passed virtual address
|
||||
* @param Address: Variable virtual address
|
||||
* @retval Data for variable or EEPROM_DEFAULT_DATA, if any errors
|
||||
*/
|
||||
uint16 EEPROMClass::read (uint16 Address)
|
||||
{
|
||||
uint16 data;
|
||||
read(Address, &data);
|
||||
return data;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the last stored variable data, if found,
|
||||
* which correspond to the passed virtual address
|
||||
* @param Address: Variable virtual address
|
||||
* @param Data: Pointer to data variable
|
||||
* @retval Success or error status:
|
||||
* - EEPROM_OK: if variable was found
|
||||
* - EEPROM_BAD_ADDRESS: if the variable was not found
|
||||
* - EEPROM_NO_VALID_PAGE: if no valid page was found.
|
||||
*/
|
||||
uint16 EEPROMClass::read(uint16 Address, uint16 *Data)
|
||||
{
|
||||
uint32 pageBase, pageEnd;
|
||||
|
||||
// Set default data (empty EEPROM)
|
||||
*Data = EEPROM_DEFAULT_DATA;
|
||||
|
||||
if (Status == EEPROM_NOT_INIT)
|
||||
if (init() != EEPROM_OK)
|
||||
return Status;
|
||||
|
||||
// Get active Page for read operation
|
||||
pageBase = EE_FindValidPage();
|
||||
if (pageBase == 0)
|
||||
return EEPROM_NO_VALID_PAGE;
|
||||
|
||||
// Get the valid Page end Address
|
||||
pageEnd = pageBase + ((uint32)(PageSize - 2));
|
||||
|
||||
// Check each active page address starting from end
|
||||
for (pageBase += 6; pageEnd >= pageBase; pageEnd -= 4)
|
||||
if ((*(__io uint16*)pageEnd) == Address) // Compare the read address with the virtual address
|
||||
{
|
||||
*Data = (*(__io uint16*)(pageEnd - 2)); // Get content of Address-2 which is variable value
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
// Return ReadStatus value: (0: variable exist, 1: variable doesn't exist)
|
||||
return EEPROM_BAD_ADDRESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Writes/upadtes variable data in EEPROM.
|
||||
* @param VirtAddress: Variable virtual address
|
||||
* @param Data: 16 bit data to be written
|
||||
* @retval Success or error status:
|
||||
* - FLASH_COMPLETE: on success
|
||||
* - EEPROM_BAD_ADDRESS: if address = 0xFFFF
|
||||
* - EEPROM_PAGE_FULL: if valid page is full
|
||||
* - EEPROM_NO_VALID_PAGE: if no valid page was found
|
||||
* - EEPROM_OUT_SIZE: if no empty EEPROM variables
|
||||
* - Flash error code: on write Flash error
|
||||
*/
|
||||
uint16 EEPROMClass::write(uint16 Address, uint16 Data)
|
||||
{
|
||||
if (Status == EEPROM_NOT_INIT)
|
||||
if (init() != EEPROM_OK)
|
||||
return Status;
|
||||
|
||||
if (Address == 0xFFFF)
|
||||
return EEPROM_BAD_ADDRESS;
|
||||
|
||||
// Write the variable virtual address and value in the EEPROM
|
||||
uint16 status = EE_VerifyPageFullWriteVariable(Address, Data);
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return number of variable
|
||||
* @retval Number of variables
|
||||
*/
|
||||
uint16 EEPROMClass::count(uint16 *Count)
|
||||
{
|
||||
if (Status == EEPROM_NOT_INIT)
|
||||
if (init() != EEPROM_OK)
|
||||
return Status;
|
||||
|
||||
// Get valid Page for write operation
|
||||
uint32 pageBase = EE_FindValidPage();
|
||||
if (pageBase == 0)
|
||||
return EEPROM_NO_VALID_PAGE; // No valid page, return max. numbers
|
||||
|
||||
*Count = EE_GetVariablesCount(pageBase, 0xFFFF);
|
||||
return EEPROM_OK;
|
||||
}
|
||||
|
||||
uint16 EEPROMClass::maxcount(void)
|
||||
{
|
||||
return ((PageSize / 4)-1);
|
||||
}
|
||||
|
||||
EEPROMClass EEPROM;
|
||||
#endif
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
#ifndef __EEPROM_H
|
||||
#define __EEPROM_H
|
||||
|
||||
#define EEPROM_USES_16BIT_WORDS
|
||||
|
||||
#include "wirish.h"
|
||||
#include "flash_stm32.h"
|
||||
|
||||
#ifndef EEPROM_PAGE_SIZE
|
||||
#if defined (MCU_STM32F103RB) || defined (MCU_STM32F103CB)
|
||||
#define EEPROM_PAGE_SIZE (uint16)0x400 /* Page size = 1KByte */
|
||||
#elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103VE) || defined (MCU_STM32F103RE)
|
||||
#define EEPROM_PAGE_SIZE (uint16)0x800 /* Page size = 2KByte */
|
||||
#elif defined (MCU_STM32F205VE) || defined (MCU_STM32F406VG)
|
||||
#define EEPROM_PAGE_SIZE (uint16)0x4000 /* Page size = 16KByte */
|
||||
#else
|
||||
#error "No MCU type specified. Add something like -DMCU_STM32F103RB " \
|
||||
"to your compiler arguments (probably in a Makefile)."
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifndef EEPROM_PAGE_SIZE
|
||||
#endif
|
||||
|
||||
#ifndef EEPROM_START_ADDRESS
|
||||
#if defined BOARD_freeflight
|
||||
#define EEPROM_START_ADDRESS ((uint32)(0x8020000 - 2 * EEPROM_PAGE_SIZE))
|
||||
#elif defined (MCU_STM32F103RB) || defined (MCU_STM32F103CB)
|
||||
#define EEPROM_START_ADDRESS ((uint32)(0x8005000 - 2 * EEPROM_PAGE_SIZE))
|
||||
#elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103VE) || defined (MCU_STM32F103RE)
|
||||
//#define EEPROM_START_ADDRESS ((uint32)(0x8080000 - 2 * EEPROM_PAGE_SIZE))
|
||||
#define EEPROM_START_ADDRESS ((uint32)(0x8005000 - 2 * EEPROM_PAGE_SIZE))
|
||||
#elif defined (MCU_STM32F205VE) || defined (MCU_STM32F406VG)
|
||||
#define EEPROM_START_ADDRESS ((uint32)(0x8010000 - 2 * EEPROM_PAGE_SIZE))
|
||||
#else
|
||||
#error "No MCU type specified. Add something like -DMCU_STM32F103RB " \
|
||||
"to your compiler arguments (probably in a Makefile)."
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* Pages 0 and 1 base and end addresses */
|
||||
#define EEPROM_PAGE0_BASE ((uint32)(EEPROM_START_ADDRESS))
|
||||
#define EEPROM_PAGE1_BASE ((uint32)(EEPROM_START_ADDRESS + EEPROM_PAGE_SIZE))
|
||||
|
||||
/* Page status definitions */
|
||||
#define EEPROM_ERASED ((uint16)0xFFFF) /* PAGE is empty */
|
||||
#define EEPROM_RECEIVE_DATA ((uint16)0xEEEE) /* PAGE is marked to receive data */
|
||||
#define EEPROM_VALID_PAGE ((uint16)0x0000) /* PAGE containing valid data */
|
||||
|
||||
/* Page full define */
|
||||
enum //: uint16
|
||||
{
|
||||
EEPROM_OK = ((uint16)0x0000),
|
||||
EEPROM_OUT_SIZE = ((uint16)0x0081),
|
||||
EEPROM_BAD_ADDRESS = ((uint16)0x0082),
|
||||
EEPROM_BAD_FLASH = ((uint16)0x0083),
|
||||
EEPROM_NOT_INIT = ((uint16)0x0084),
|
||||
EEPROM_NO_VALID_PAGE = ((uint16)0x00AB)
|
||||
};
|
||||
|
||||
#define EEPROM_DEFAULT_DATA 0xFFFF
|
||||
|
||||
|
||||
class EEPROMClass
|
||||
{
|
||||
public:
|
||||
EEPROMClass(void);
|
||||
|
||||
uint16 init(void);
|
||||
uint16 init(uint32, uint32, uint32);
|
||||
|
||||
uint16 format(void);
|
||||
|
||||
uint16 erases(uint16 *);
|
||||
uint16 read (uint16 address);
|
||||
uint16 read (uint16 address, uint16 *data);
|
||||
uint16 write(uint16 address, uint16 data);
|
||||
uint16 count(uint16 *);
|
||||
uint16 maxcount(void);
|
||||
|
||||
uint32 PageBase0;
|
||||
uint32 PageBase1;
|
||||
uint32 PageSize;
|
||||
uint16 Status;
|
||||
private:
|
||||
FLASH_Status EE_ErasePage(uint32);
|
||||
|
||||
uint16 EE_CheckPage(uint32, uint16);
|
||||
uint16 EE_CheckErasePage(uint32, uint16);
|
||||
uint16 EE_Format(void);
|
||||
uint32 EE_FindValidPage(void);
|
||||
uint16 EE_GetVariablesCount(uint32, uint16);
|
||||
uint16 EE_PageTransfer(uint32, uint32, uint16);
|
||||
uint16 EE_VerifyPageFullWriteVariable(uint16, uint16);
|
||||
};
|
||||
|
||||
extern EEPROMClass EEPROM;
|
||||
|
||||
#endif /* __EEPROM_H */
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#if defined (MCU_STM32F205VE) || defined (MCU_STM32F406VG)
|
||||
#include "flash_stm32F2.h"
|
||||
#else
|
||||
#include "flash_stm32F1.h"
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
#ifndef __FLASH_STM32_H
|
||||
#define __FLASH_STM32_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
typedef enum
|
||||
{
|
||||
FLASH_BUSY = 1,
|
||||
FLASH_ERROR_PG,
|
||||
FLASH_ERROR_WRP,
|
||||
FLASH_ERROR_OPT,
|
||||
FLASH_COMPLETE,
|
||||
FLASH_TIMEOUT,
|
||||
FLASH_BAD_ADDRESS,
|
||||
FLASH_ERROR_PGS,
|
||||
FLASH_ERROR_PGP,
|
||||
FLASH_ERROR_PGA,
|
||||
FLASH_ERROR_PROGRAM,
|
||||
FLASH_ERROR_OPERATION
|
||||
} FLASH_Status;
|
||||
|
||||
#define IS_FLASH_ADDRESS(ADDRESS) (((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x0807FFFF))
|
||||
|
||||
FLASH_Status FLASH_WaitForLastOperation(uint32 Timeout);
|
||||
FLASH_Status FLASH_ErasePage(uint32 Page_Address);
|
||||
FLASH_Status FLASH_ProgramHalfWord(uint32 Address, uint16 Data);
|
||||
void FLASH_Unlock(void);
|
||||
void FLASH_Lock(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __FLASH_STM32_H */
|
||||
|
|
@ -0,0 +1,208 @@
|
|||
#include <AP_HAL_Boards.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
||||
#include "libmaple.h"
|
||||
#include "util.h"
|
||||
#include "flash.h"
|
||||
#include "flash_stm32.h"
|
||||
|
||||
#ifndef __get_bits
|
||||
// add macros missing in maple 0.0.12
|
||||
#define __set_bits(addr, mask) (*(volatile uint32*)(addr) |= (uint32)(mask))
|
||||
#define __clear_bits(addr, mask) (*(volatile uint32*)(addr) &= (uint32)~(mask))
|
||||
#define __get_bits(addr, mask) (*(volatile uint32*)(addr) & (uint32)(mask))
|
||||
|
||||
#define __read(reg) (*(volatile uint32*)(reg))
|
||||
#define __write(reg, value) (*(volatile uint32*)(reg) = (value))
|
||||
#endif
|
||||
|
||||
#undef FLASH_BASE
|
||||
#define FLASH_BASE 0x40022000
|
||||
#define FLASH_SR (FLASH_BASE + 0x0C)
|
||||
// FLASH_ACR + 0x00
|
||||
#define FLASH_KEYR (FLASH_BASE + 0x04)
|
||||
// FLASH_OPTKEYR + 0x08
|
||||
#define FLASH_CR (FLASH_BASE + 0x10)
|
||||
#define FLASH_AR (FLASH_BASE + 0x14)
|
||||
// FLASH_RESERVED + 0x18
|
||||
// FLASH_OBR + 0x1C
|
||||
// FLASH_WRPR + 0x20
|
||||
|
||||
#define FLASH_FLAG_BSY ((uint32)0x00000001) /*!< FLASH Busy flag */
|
||||
#define FLASH_FLAG_PGERR ((uint32)0x00000004) /*!< FLASH Program error flag */
|
||||
#define FLASH_FLAG_WRPRTERR ((uint32)0x00000010) /*!< FLASH Write protected error flag */
|
||||
#define FLASH_FLAG_EOP ((uint32)0x00000020) /*!< FLASH End of Operation flag */
|
||||
#define FLASH_FLAG_OPTERR ((uint32)0x00000001) /*!< FLASH Option Byte error flag */
|
||||
|
||||
/* Flash Control Register bits */
|
||||
#define CR_PG_Set ((uint32)0x00000001)
|
||||
#define CR_PG_Reset ((uint32)0x00001FFE)
|
||||
#define CR_PER_Set ((uint32)0x00000002)
|
||||
#define CR_PER_Reset ((uint32)0x00001FFD)
|
||||
//#define CR_MER_Set ((uint32)0x00000004)
|
||||
//#define CR_MER_Reset ((uint32)0x00001FFB)
|
||||
//#define CR_OPTPG_Set ((uint32)0x00000010)
|
||||
//#define CR_OPTPG_Reset ((uint32)0x00001FEF)
|
||||
//#define CR_OPTER_Set ((uint32)0x00000020)
|
||||
//#define CR_OPTER_Reset ((uint32)0x00001FDF)
|
||||
#define CR_STRT_Set ((uint32)0x00000040)
|
||||
#define CR_LOCK_Set ((uint32)0x00000080)
|
||||
|
||||
#define FLASH_KEY1 ((uint32)0x45670123)
|
||||
#define FLASH_KEY2 ((uint32)0xCDEF89AB)
|
||||
|
||||
/* Delay definition */
|
||||
#define EraseTimeout ((uint32)0x00000FFF)
|
||||
#define ProgramTimeout ((uint32)0x0000001F)
|
||||
|
||||
/**
|
||||
* @brief Inserts a time delay.
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void delay(void)
|
||||
{
|
||||
__io uint32 i = 0;
|
||||
for(i = 0xFF; i != 0; i--) { }
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the FLASH Status.
|
||||
* @param None
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP or FLASH_COMPLETE
|
||||
*/
|
||||
FLASH_Status FLASH_GetStatus(void)
|
||||
{
|
||||
if(__get_bits(FLASH_SR, FLASH_FLAG_BSY) == FLASH_FLAG_BSY)
|
||||
return FLASH_BUSY;
|
||||
|
||||
if(__get_bits(FLASH_SR, FLASH_FLAG_PGERR) != 0)
|
||||
return FLASH_ERROR_PG;
|
||||
|
||||
if(__get_bits(FLASH_SR, FLASH_FLAG_WRPRTERR) != 0 )
|
||||
return FLASH_ERROR_WRP;
|
||||
|
||||
if(__get_bits(FLASH_SR, FLASH_FLAG_OPTERR) != 0 )
|
||||
return FLASH_ERROR_OPT;
|
||||
|
||||
return FLASH_COMPLETE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits for a Flash operation to complete or a TIMEOUT to occur.
|
||||
* @param Timeout: FLASH progamming Timeout
|
||||
* @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
|
||||
*/
|
||||
FLASH_Status FLASH_WaitForLastOperation(uint32 Timeout)
|
||||
{
|
||||
FLASH_Status status;
|
||||
|
||||
/* Check for the Flash Status */
|
||||
status = FLASH_GetStatus();
|
||||
/* Wait for a Flash operation to complete or a TIMEOUT to occur */
|
||||
while((status == FLASH_BUSY) && (Timeout != 0x00))
|
||||
{
|
||||
delay();
|
||||
status = FLASH_GetStatus();
|
||||
Timeout--;
|
||||
}
|
||||
if (Timeout == 0)
|
||||
status = FLASH_TIMEOUT;
|
||||
/* Return the operation status */
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Erases a specified FLASH page.
|
||||
* @param Page_Address: The page address to be erased.
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
|
||||
*/
|
||||
FLASH_Status FLASH_ErasePage(uint32 Page_Address)
|
||||
{
|
||||
FLASH_Status status = FLASH_COMPLETE;
|
||||
/* Check the parameters */
|
||||
ASSERT(IS_FLASH_ADDRESS(Page_Address));
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(EraseTimeout);
|
||||
|
||||
if(status == FLASH_COMPLETE)
|
||||
{
|
||||
/* if the previous operation is completed, proceed to erase the page */
|
||||
__set_bits(FLASH_CR, CR_PER_Set);
|
||||
__write(FLASH_AR, Page_Address);
|
||||
__set_bits(FLASH_CR, CR_STRT_Set);
|
||||
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(EraseTimeout);
|
||||
if(status != FLASH_TIMEOUT)
|
||||
{
|
||||
/* if the erase operation is completed, disable the PER Bit */
|
||||
__clear_bits(FLASH_CR, ~CR_PER_Reset);
|
||||
}
|
||||
__write(FLASH_SR, (FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR));
|
||||
}
|
||||
/* Return the Erase Status */
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Programs a half word at a specified address.
|
||||
* @param Address: specifies the address to be programmed.
|
||||
* @param Data: specifies the data to be programmed.
|
||||
* @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
|
||||
*/
|
||||
FLASH_Status FLASH_ProgramHalfWord(uint32 Address, uint16 Data)
|
||||
{
|
||||
FLASH_Status status = FLASH_BAD_ADDRESS;
|
||||
|
||||
if (IS_FLASH_ADDRESS(Address))
|
||||
{
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(ProgramTimeout);
|
||||
if(status == FLASH_COMPLETE)
|
||||
{
|
||||
/* if the previous operation is completed, proceed to program the new data */
|
||||
__set_bits(FLASH_CR, CR_PG_Set);
|
||||
*(__io uint16*)Address = Data;
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(ProgramTimeout);
|
||||
if(status != FLASH_TIMEOUT)
|
||||
{
|
||||
/* if the program operation is completed, disable the PG Bit */
|
||||
__clear_bits(FLASH_CR, ~CR_PG_Reset);
|
||||
}
|
||||
__write(FLASH_SR, (FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR));
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Unlocks the FLASH Program Erase Controller.
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void FLASH_Unlock(void)
|
||||
{
|
||||
/* Authorize the FPEC Access */
|
||||
__write(FLASH_KEYR, FLASH_KEY1);
|
||||
__write(FLASH_KEYR, FLASH_KEY2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Locks the FLASH Program Erase Controller.
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void FLASH_Lock(void)
|
||||
{
|
||||
/* Set the Lock Bit to lock the FPEC and the FCR */
|
||||
__set_bits(FLASH_CR, CR_LOCK_Set);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,311 @@
|
|||
#include "libmaple.h"
|
||||
#include "util.h"
|
||||
//#include "flash.h"
|
||||
#include "flash_stm32.h"
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
__io uint32 ACR; /*!< FLASH access control register, Address offset: 0x00 */
|
||||
__io uint32 KEYR; /*!< FLASH key register, Address offset: 0x04 */
|
||||
__io uint32 OPTKEYR; /*!< FLASH option key register, Address offset: 0x08 */
|
||||
__io uint32 SR; /*!< FLASH status register, Address offset: 0x0C */
|
||||
__io uint32 CR; /*!< FLASH control register, Address offset: 0x10 */
|
||||
__io uint32 OPTCR; /*!< FLASH option control register, Address offset: 0x14 */
|
||||
} FLASH_TypeDef;
|
||||
|
||||
#define PERIPH_BASE ((uint32)0x40000000) /*!< Peripheral base address in the alias region */
|
||||
#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000)
|
||||
#define FLASH_R_BASE (AHB1PERIPH_BASE + 0x3C00)
|
||||
#define FLASH ((FLASH_TypeDef *) FLASH_R_BASE)
|
||||
|
||||
#define FLASH_FLAG_EOP ((uint32)0x00000001) /*!< FLASH End of Operation flag */
|
||||
#define FLASH_FLAG_OPERR ((uint32)0x00000002) /*!< FLASH operation Error flag */
|
||||
#define FLASH_FLAG_WRPERR ((uint32)0x00000010) /*!< FLASH Write protected error flag */
|
||||
#define FLASH_FLAG_PGAERR ((uint32)0x00000020) /*!< FLASH Programming Alignment error flag */
|
||||
#define FLASH_FLAG_PGPERR ((uint32)0x00000040) /*!< FLASH Programming Parallelism error flag */
|
||||
#define FLASH_FLAG_PGSERR ((uint32)0x00000080) /*!< FLASH Programming Sequence error flag */
|
||||
#define FLASH_FLAG_BSY ((uint32)0x00010000) /*!< FLASH Busy flag */
|
||||
|
||||
#define FLASH_PSIZE_BYTE ((uint32)0x00000000)
|
||||
#define FLASH_PSIZE_HALF_WORD ((uint32)0x00000100)
|
||||
#define FLASH_PSIZE_WORD ((uint32)0x00000200)
|
||||
#define FLASH_PSIZE_DOUBLE_WORD ((uint32)0x00000300)
|
||||
#define CR_PSIZE_MASK ((uint32)0xFFFFFCFF)
|
||||
|
||||
#define SECTOR_MASK ((uint32)0xFFFFFF07)
|
||||
|
||||
/******************* Bits definition for FLASH_CR register ******************/
|
||||
#define FLASH_CR_PG ((uint32)0x00000001)
|
||||
#define FLASH_CR_SER ((uint32)0x00000002)
|
||||
#define FLASH_CR_MER ((uint32)0x00000004)
|
||||
#define FLASH_CR_SNB_0 ((uint32)0x00000008)
|
||||
#define FLASH_CR_SNB_1 ((uint32)0x00000010)
|
||||
#define FLASH_CR_SNB_2 ((uint32)0x00000020)
|
||||
#define FLASH_CR_SNB_3 ((uint32)0x00000040)
|
||||
#define FLASH_CR_PSIZE_0 ((uint32)0x00000100)
|
||||
#define FLASH_CR_PSIZE_1 ((uint32)0x00000200)
|
||||
#define FLASH_CR_STRT ((uint32)0x00010000)
|
||||
#define FLASH_CR_EOPIE ((uint32)0x01000000)
|
||||
#define FLASH_CR_LOCK ((uint32)0x80000000)
|
||||
|
||||
#define FLASH_KEY1 ((uint32)0x45670123)
|
||||
#define FLASH_KEY2 ((uint32)0xCDEF89AB)
|
||||
|
||||
/* Delay definition */
|
||||
#define EraseTimeout ((uint32)0x00000FFF)
|
||||
#define ProgramTimeout ((uint32)0x0000001F)
|
||||
|
||||
#define VoltageRange_1 ((uint8)0x00) /*!< Device operating range: 1.8V to 2.1V */
|
||||
#define VoltageRange_2 ((uint8)0x01) /*!<Device operating range: 2.1V to 2.7V */
|
||||
#define VoltageRange_3 ((uint8)0x02) /*!<Device operating range: 2.7V to 3.6V */
|
||||
#define VoltageRange_4 ((uint8)0x03) /*!<Device operating range: 2.7V to 3.6V + External Vpp */
|
||||
|
||||
#define IS_VOLTAGERANGE(RANGE)(((RANGE) == VoltageRange_1) || \
|
||||
((RANGE) == VoltageRange_2) || \
|
||||
((RANGE) == VoltageRange_3) || \
|
||||
((RANGE) == VoltageRange_4))
|
||||
|
||||
/**
|
||||
* @brief Inserts a time delay.
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void delay(void)
|
||||
{
|
||||
__io uint32 i = 0;
|
||||
for(i = 0xFF; i != 0; i--) { }
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the FLASH Status.
|
||||
* @param None
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PROGRAM,
|
||||
* FLASH_ERROR_WRP, FLASH_ERROR_OPERATION or FLASH_COMPLETE.
|
||||
*/
|
||||
FLASH_Status FLASH_GetStatus(void)
|
||||
{
|
||||
FLASH_Status flashstatus = FLASH_COMPLETE;
|
||||
|
||||
if((FLASH->SR & FLASH_FLAG_BSY) == FLASH_FLAG_BSY)
|
||||
{
|
||||
flashstatus = FLASH_BUSY;
|
||||
}
|
||||
else
|
||||
{
|
||||
if((FLASH->SR & FLASH_FLAG_WRPERR) != 0)
|
||||
{
|
||||
flashstatus = FLASH_ERROR_WRP;
|
||||
}
|
||||
else
|
||||
{
|
||||
if((FLASH->SR & 0xEF) != 0)
|
||||
{
|
||||
flashstatus = FLASH_ERROR_PROGRAM;
|
||||
}
|
||||
else
|
||||
{
|
||||
if((FLASH->SR & FLASH_FLAG_OPERR) != 0)
|
||||
{
|
||||
flashstatus = FLASH_ERROR_OPERATION;
|
||||
}
|
||||
else
|
||||
{
|
||||
flashstatus = FLASH_COMPLETE;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Return the FLASH Status */
|
||||
return flashstatus;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits for a Flash operation to complete or a TIMEOUT to occur.
|
||||
* @param Timeout: FLASH progamming Timeout
|
||||
* @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
|
||||
*/
|
||||
FLASH_Status FLASH_WaitForLastOperation(uint32 Timeout)
|
||||
{
|
||||
FLASH_Status status;
|
||||
|
||||
/* Check for the Flash Status */
|
||||
status = FLASH_GetStatus();
|
||||
/* Wait for a Flash operation to complete or a TIMEOUT to occur */
|
||||
while((status == FLASH_BUSY) && (Timeout != 0x00))
|
||||
{
|
||||
delay();
|
||||
status = FLASH_GetStatus();
|
||||
Timeout--;
|
||||
}
|
||||
if (Timeout == 0)
|
||||
status = FLASH_TIMEOUT;
|
||||
/* Return the operation status */
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Erases a specified FLASH Sector.
|
||||
*
|
||||
* @param FLASH_Sector: The Sector number to be erased.
|
||||
* This parameter can be a value between FLASH_Sector_0 and FLASH_Sector_11
|
||||
*
|
||||
* @param VoltageRange: The device voltage range which defines the erase parallelism.
|
||||
* This parameter can be one of the following values:
|
||||
* @arg VoltageRange_1: when the device voltage range is 1.8V to 2.1V,
|
||||
* the operation will be done by byte (8-bit)
|
||||
* @arg VoltageRange_2: when the device voltage range is 2.1V to 2.7V,
|
||||
* the operation will be done by half word (16-bit)
|
||||
* @arg VoltageRange_3: when the device voltage range is 2.7V to 3.6V,
|
||||
* the operation will be done by word (32-bit)
|
||||
* @arg VoltageRange_4: when the device voltage range is 2.7V to 3.6V + External Vpp,
|
||||
* the operation will be done by double word (64-bit)
|
||||
*
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PROGRAM,
|
||||
* FLASH_ERROR_WRP, FLASH_ERROR_OPERATION or FLASH_COMPLETE.
|
||||
*/
|
||||
FLASH_Status FLASH_EraseSector(uint32 FLASH_Sector, uint8 VoltageRange)
|
||||
{
|
||||
uint32 tmp_psize = 0x0;
|
||||
FLASH_Status status = FLASH_COMPLETE;
|
||||
|
||||
/* Check the parameters */
|
||||
//assert_param(IS_FLASH_SECTOR(FLASH_Sector));
|
||||
//assert_param(IS_VOLTAGERANGE(VoltageRange));
|
||||
|
||||
if(VoltageRange == VoltageRange_1)
|
||||
{
|
||||
tmp_psize = FLASH_PSIZE_BYTE;
|
||||
}
|
||||
else if(VoltageRange == VoltageRange_2)
|
||||
{
|
||||
tmp_psize = FLASH_PSIZE_HALF_WORD;
|
||||
}
|
||||
else if(VoltageRange == VoltageRange_3)
|
||||
{
|
||||
tmp_psize = FLASH_PSIZE_WORD;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp_psize = FLASH_PSIZE_DOUBLE_WORD;
|
||||
}
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(EraseTimeout);
|
||||
|
||||
if(status == FLASH_COMPLETE)
|
||||
{
|
||||
/* if the previous operation is completed, proceed to erase the sector */
|
||||
FLASH->CR &= CR_PSIZE_MASK;
|
||||
FLASH->CR |= tmp_psize;
|
||||
FLASH->CR &= SECTOR_MASK;
|
||||
FLASH->CR |= FLASH_CR_SER | FLASH_Sector;
|
||||
FLASH->CR |= FLASH_CR_STRT;
|
||||
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(EraseTimeout);
|
||||
|
||||
/* if the erase operation is completed, disable the SER Bit */
|
||||
FLASH->CR &= (~FLASH_CR_SER);
|
||||
FLASH->CR &= SECTOR_MASK;
|
||||
}
|
||||
/* Return the Erase Status */
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Erases a specified FLASH page.
|
||||
* @param Page_Address: The page address to be erased.
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PG,
|
||||
* FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
|
||||
*/
|
||||
FLASH_Status FLASH_ErasePage(uint32 Page_Address)
|
||||
{
|
||||
int Page_Offset = Page_Address - 0x08000000;
|
||||
uint32 FLASH_Sector;
|
||||
|
||||
if(Page_Offset < 0x10000) {
|
||||
FLASH_Sector = Page_Offset / 0x4000;
|
||||
} else if(Page_Offset < 0x20000) {
|
||||
FLASH_Sector = 4;
|
||||
} else {
|
||||
FLASH_Sector = 4 + Page_Offset / 0x20000;
|
||||
}
|
||||
|
||||
return FLASH_EraseSector(8 * FLASH_Sector, VoltageRange_4);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Programs a half word (16-bit) at a specified address.
|
||||
* @note This function must be used when the device voltage range is from 2.1V to 3.6V.
|
||||
* @param Address: specifies the address to be programmed.
|
||||
* This parameter can be any address in Program memory zone or in OTP zone.
|
||||
* @param Data: specifies the data to be programmed.
|
||||
* @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PROGRAM,
|
||||
* FLASH_ERROR_WRP, FLASH_ERROR_OPERATION or FLASH_COMPLETE.
|
||||
*/
|
||||
FLASH_Status FLASH_ProgramHalfWord(uint32 Address, uint16 Data)
|
||||
{
|
||||
FLASH_Status status = FLASH_BAD_ADDRESS;
|
||||
|
||||
if (IS_FLASH_ADDRESS(Address))
|
||||
{
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(ProgramTimeout);
|
||||
|
||||
if(status == FLASH_COMPLETE)
|
||||
{
|
||||
/* if the previous operation is completed, proceed to program the new data */
|
||||
FLASH->CR &= CR_PSIZE_MASK;
|
||||
FLASH->CR |= FLASH_PSIZE_HALF_WORD;
|
||||
FLASH->CR |= FLASH_CR_PG;
|
||||
|
||||
*(__io uint16*)Address = Data;
|
||||
|
||||
/* Wait for last operation to be completed */
|
||||
status = FLASH_WaitForLastOperation(ProgramTimeout);
|
||||
|
||||
if(status != FLASH_TIMEOUT)
|
||||
{
|
||||
/* if the program operation is completed, disable the PG Bit */
|
||||
FLASH->CR &= (~FLASH_CR_PG);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Return the Program Status */
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Unlocks the FLASH control register access
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void FLASH_Unlock(void)
|
||||
{
|
||||
if((FLASH->CR & FLASH_CR_LOCK) != 0)
|
||||
{
|
||||
/* Authorize the FLASH Registers access */
|
||||
FLASH->KEYR = FLASH_KEY1;
|
||||
FLASH->KEYR = FLASH_KEY2;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Locks the FLASH control register access
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void FLASH_Lock(void)
|
||||
{
|
||||
/* Set the LOCK Bit to lock the FLASH Registers access */
|
||||
FLASH->CR |= FLASH_CR_LOCK;
|
||||
}
|
||||
Loading…
Reference in New Issue