Skip to content

Commit

Permalink
#368 Modified the deck API to use a type for deck pins to avoid mixup…
Browse files Browse the repository at this point in the history
… with pin definitions for the CPU. This change will catch errors at compile time.
  • Loading branch information
krichardsson committed Aug 19, 2020
1 parent 3263307 commit b8ba12c
Show file tree
Hide file tree
Showing 11 changed files with 93 additions and 75 deletions.
16 changes: 7 additions & 9 deletions src/deck/api/deck_analog.c
Original file line number Diff line number Diff line change
Expand Up @@ -80,31 +80,30 @@ static uint16_t analogReadChannel(uint8_t channel)
return ADC_GetConversionValue(ADC2);
}

uint16_t analogRead(uint32_t pin)
uint16_t analogRead(const deckPin_t pin)
{
assert_param((pin >= 1) && (pin <= 13));
assert_param(deckGPIOMapping[pin-1].adcCh > -1);
assert_param(deckGPIOMapping[pin.id].adcCh > -1);

/* Now set the GPIO pin to analog mode. */

/* Enable clock for the peripheral of the pin.*/
RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin-1].periph, ENABLE);
RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);

/* Populate structure with RESET values. */
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);

/* Initialise the GPIO pin to analog mode. */
GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin-1].pin;
GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;

/* TODO: Any settling time before we can do ADC after init on the GPIO pin? */
GPIO_Init(deckGPIOMapping[pin-1].port, &GPIO_InitStructure);
GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);

/* Read the appropriate ADC channel. */
return analogReadChannel((uint8_t)deckGPIOMapping[pin-1].adcCh);
return analogReadChannel((uint8_t)deckGPIOMapping[pin.id].adcCh);
}

void analogReference(uint8_t type)
Expand Down Expand Up @@ -143,12 +142,11 @@ void analogReadResolution(uint8_t bits)
ADC_Init(ADC2, &ADC_InitStructure);
}

float analogReadVoltage(uint32_t pin)
float analogReadVoltage(const deckPin_t pin)
{
float voltage;

voltage = analogRead(pin) * VREF / adcRange;

return voltage;

}
15 changes: 15 additions & 0 deletions src/deck/api/deck_constants.c
Original file line number Diff line number Diff line change
Expand Up @@ -42,3 +42,18 @@ deckGPIOMapping_t deckGPIOMapping[13] = {
{.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_6, .adcCh=ADC_Channel_6}, /* MISO */
{.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_7, .adcCh=ADC_Channel_7}, /* MOSI */
};

// Pin definitions
const deckPin_t DECK_GPIO_RX1 = {.id=0};
const deckPin_t DECK_GPIO_TX1 = {.id=1};
const deckPin_t DECK_GPIO_SDA = {.id=2};
const deckPin_t DECK_GPIO_SCL = {.id=3};
const deckPin_t DECK_GPIO_IO1 = {.id=4};
const deckPin_t DECK_GPIO_IO2 = {.id=5};
const deckPin_t DECK_GPIO_IO3 = {.id=6};
const deckPin_t DECK_GPIO_IO4 = {.id=7};
const deckPin_t DECK_GPIO_TX2 = {.id=8};
const deckPin_t DECK_GPIO_RX2 = {.id=9};
const deckPin_t DECK_GPIO_SCK = {.id=10};
const deckPin_t DECK_GPIO_MISO = {.id=11};
const deckPin_t DECK_GPIO_MOSI = {.id=12};
31 changes: 11 additions & 20 deletions src/deck/api/deck_digital.c
Original file line number Diff line number Diff line change
Expand Up @@ -28,42 +28,33 @@

#include "stm32fxxx.h"

void pinMode(uint32_t pin, uint32_t mode)
void pinMode(const deckPin_t pin, const uint32_t mode)
{
if (pin > 13) {
return;
}

RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin-1].periph, ENABLE);
RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);

GPIO_InitTypeDef GPIO_InitStructure = {0};

GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin-1].pin;
GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
GPIO_InitStructure.GPIO_Mode = (mode == OUTPUT) ? GPIO_Mode_OUT:GPIO_Mode_IN;
if (mode == OUTPUT) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
if (mode == INPUT_PULLUP) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
if (mode == INPUT_PULLDOWN) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_Init(deckGPIOMapping[pin-1].port, &GPIO_InitStructure);
GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);
}

void digitalWrite(uint32_t pin, uint32_t val)
void digitalWrite(const deckPin_t pin, const uint32_t val)
{
if (pin > 13) {
return;
BitAction action = Bit_RESET;
if (val) {
action = Bit_SET;
}

if (val) val = Bit_SET;

GPIO_WriteBit(deckGPIOMapping[pin-1].port, deckGPIOMapping[pin-1].pin, val);
GPIO_WriteBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin, action);
}

int digitalRead(uint32_t pin)
int digitalRead(const deckPin_t pin)
{
if (pin > 13) {
return LOW;
}

int val = GPIO_ReadInputDataBit(deckGPIOMapping[pin-1].port, deckGPIOMapping[pin-1].pin);
int val = GPIO_ReadInputDataBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin);
return (val==Bit_SET)?HIGH:LOW;
}
5 changes: 3 additions & 2 deletions src/deck/interface/deck_analog.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,15 @@
#define __DECK_ANALOG_H__

#include <stdint.h>
#include "deck_constants.h"

/* Voltage reference types for the analogReference() function. */
#define DEFAULT 0
#define VREF 3.0

void adcInit(void);

uint16_t analogRead(uint32_t pin);
uint16_t analogRead(const deckPin_t pin);

void analogReference(uint8_t type);

Expand All @@ -46,6 +47,6 @@ void analogReadResolution(uint8_t bits);
* @param[in] pin deck pin to measure.
* @return voltage in volts
*/
float analogReadVoltage(uint32_t pin);
float analogReadVoltage(const deckPin_t pin);

#endif
32 changes: 19 additions & 13 deletions src/deck/interface/deck_constants.h
Original file line number Diff line number Diff line change
Expand Up @@ -44,19 +44,25 @@
* The Deck port GPIO pins, as named by the deck port / expansion port / expansion breakout documentation on bitcraze.io.
* Sequenced according to the deckGPIOMapping struct, so that these can be used for lookup in the struct.
*/
#define DECK_GPIO_RX1 1
#define DECK_GPIO_TX1 2
#define DECK_GPIO_SDA 3
#define DECK_GPIO_SCL 4
#define DECK_GPIO_IO1 5
#define DECK_GPIO_IO2 6
#define DECK_GPIO_IO3 7
#define DECK_GPIO_IO4 8
#define DECK_GPIO_TX2 9
#define DECK_GPIO_RX2 10
#define DECK_GPIO_SCK 11
#define DECK_GPIO_MISO 12
#define DECK_GPIO_MOSI 13

// Type used to identify a pin in the deck API.
// id is used as an index in the deckGPIOMapping array
typedef struct {uint8_t id;} deckPin_t;

extern const deckPin_t DECK_GPIO_RX1;
extern const deckPin_t DECK_GPIO_TX1;
extern const deckPin_t DECK_GPIO_SDA;
extern const deckPin_t DECK_GPIO_SCL;
extern const deckPin_t DECK_GPIO_IO1;
extern const deckPin_t DECK_GPIO_IO2;
extern const deckPin_t DECK_GPIO_IO3;
extern const deckPin_t DECK_GPIO_IO4;
extern const deckPin_t DECK_GPIO_TX2;
extern const deckPin_t DECK_GPIO_RX2;
extern const deckPin_t DECK_GPIO_SCK;
extern const deckPin_t DECK_GPIO_MISO;
extern const deckPin_t DECK_GPIO_MOSI;


typedef const struct {
uint32_t periph;
Expand Down
7 changes: 4 additions & 3 deletions src/deck/interface/deck_digital.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,12 @@
#define __DECK_DIGITAL_H__

#include <stdint.h>
#include "deck_constants.h"

void pinMode(uint32_t pin, uint32_t mode);
void pinMode(const deckPin_t pin, const uint32_t mode);

void digitalWrite(uint32_t pin, uint32_t val);
void digitalWrite(const deckPin_t pin, const uint32_t val);

int digitalRead(uint32_t pin);
int digitalRead(const deckPin_t pin);

#endif
5 changes: 3 additions & 2 deletions src/drivers/interface/pmw3901.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#define PMW3901_H_

#include <stdint.h>
#include "deck.h"

typedef struct motionBurst_s {
union {
Expand Down Expand Up @@ -62,15 +63,15 @@ typedef struct motionBurst_s {
*
* @return true if init successful else false.
*/
bool pmw3901Init(uint32_t csPin);
bool pmw3901Init(const deckPin_t csPin);

/**
* Read the current accumulated motion.
*
* @param csPin Chip Select pin as defined in deck pinout driver.
* @param motion A filled in motionBurst_t structure with the latest motion information
*/
void pmw3901ReadMotion(uint32_t csPin, motionBurst_t * motion);
void pmw3901ReadMotion(const deckPin_t csPin, motionBurst_t * motion);


#endif /* PMW3901_H_ */
2 changes: 1 addition & 1 deletion src/drivers/src/maxsonar.c
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ static uint32_t maxSonarGetAccuracyMB1040(uint32_t distance)
*
* @return The distance measurement in millimeters.
*/
static uint32_t maxSonarReadDistanceMB1040AN(uint8_t pin, uint32_t *accuracy)
static uint32_t maxSonarReadDistanceMB1040AN(const deckPin_t pin, uint32_t *accuracy)
{
/*
* analogRead() returns a 12-bit (0-4095) value scaled to the range between GND (0V) and VREF.
Expand Down
10 changes: 5 additions & 5 deletions src/drivers/src/pmw3901.c
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

static bool isInit = false;

static void registerWrite(uint32_t csPin, uint8_t reg, uint8_t value)
static void registerWrite(const deckPin_t csPin, uint8_t reg, uint8_t value)
{
// Set MSB to 1 for write
reg |= 0x80u;
Expand All @@ -60,7 +60,7 @@ static void registerWrite(uint32_t csPin, uint8_t reg, uint8_t value)
sleepus(200);
}

static uint8_t registerRead(uint32_t csPin, uint8_t reg)
static uint8_t registerRead(const deckPin_t csPin, uint8_t reg)
{
uint8_t data = 0;
uint8_t dummy = 0;
Expand All @@ -86,7 +86,7 @@ static uint8_t registerRead(uint32_t csPin, uint8_t reg)
return data;
}

static void InitRegisters(uint32_t csPin)
static void InitRegisters(const deckPin_t csPin)
{
registerWrite(csPin, 0x7F, 0x00);
registerWrite(csPin, 0x61, 0xAD);
Expand Down Expand Up @@ -170,7 +170,7 @@ static void InitRegisters(uint32_t csPin)
registerWrite(csPin, 0x54, 0x00);
}

bool pmw3901Init(uint32_t csPin)
bool pmw3901Init(const deckPin_t csPin)
{
if (isInit) {
return true;
Expand Down Expand Up @@ -217,7 +217,7 @@ bool pmw3901Init(uint32_t csPin)
return isInit;
}

void pmw3901ReadMotion(uint32_t csPin, motionBurst_t * motion)
void pmw3901ReadMotion(const deckPin_t csPin, motionBurst_t * motion)
{
uint8_t address = 0x16;

Expand Down
9 changes: 5 additions & 4 deletions src/hal/interface/pm.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
Expand Down Expand Up @@ -29,6 +29,7 @@

#include "adc.h"
#include "syslink.h"
#include "deck.h"

#ifndef CRITICAL_LOW_VOLTAGE
#define PM_BAT_CRITICAL_LOW_VOLTAGE 3.0f
Expand Down Expand Up @@ -141,7 +142,7 @@ bool pmIsDischarging(void);
/**
* Enable or disable external battery voltage measuring.
*/
void pmEnableExtBatteryVoltMeasuring(uint8_t pin, float multiplier);
void pmEnableExtBatteryVoltMeasuring(const deckPin_t pin, float multiplier);

/**
* Measure an external voltage.
Expand All @@ -151,7 +152,7 @@ float pmMeasureExtBatteryVoltage(void);
/**
* Enable or disable external battery current measuring.
*/
void pmEnableExtBatteryCurrMeasuring(uint8_t pin, float ampPerVolt);
void pmEnableExtBatteryCurrMeasuring(const deckPin_t pin, float ampPerVolt);

/**
* Measure an external current.
Expand Down
36 changes: 20 additions & 16 deletions src/hal/src/pm_stm32f4.c
Original file line number Diff line number Diff line change
Expand Up @@ -61,18 +61,20 @@ typedef struct _PmSyslinkInfo
#endif
} __attribute__((packed)) PmSyslinkInfo;

static float batteryVoltage;
static uint16_t batteryVoltageMV;
static float batteryVoltageMin = 6.0;
static float batteryVoltageMax = 0.0;

static float extBatteryVoltage;
static uint16_t extBatteryVoltageMV;
static uint8_t extBatVoltDeckPin;
static float extBatVoltMultiplier;
static float extBatteryCurrent;
static uint8_t extBatCurrDeckPin;
static float extBatCurrAmpPerVolt;
static float batteryVoltage;
static uint16_t batteryVoltageMV;
static float batteryVoltageMin = 6.0;
static float batteryVoltageMax = 0.0;

static float extBatteryVoltage;
static uint16_t extBatteryVoltageMV;
static deckPin_t extBatVoltDeckPin;
static bool isExtBatVoltDeckPinSet = false;
static float extBatVoltMultiplier;
static float extBatteryCurrent;
static deckPin_t extBatCurrDeckPin;
static bool isExtBatCurrDeckPinSet = false;
static float extBatCurrAmpPerVolt;

#ifdef PM_SYSTLINK_INLCUDE_TEMP
// nRF51 internal temp
Expand Down Expand Up @@ -236,17 +238,18 @@ PMStates pmUpdateState()
return state;
}

void pmEnableExtBatteryCurrMeasuring(uint8_t pin, float ampPerVolt)
void pmEnableExtBatteryCurrMeasuring(const deckPin_t pin, float ampPerVolt)
{
extBatCurrDeckPin = pin;
isExtBatCurrDeckPinSet = true;
extBatCurrAmpPerVolt = ampPerVolt;
}

float pmMeasureExtBatteryCurrent(void)
{
float current;

if (extBatCurrDeckPin)
if (isExtBatCurrDeckPinSet)
{
current = analogReadVoltage(extBatCurrDeckPin) * extBatCurrAmpPerVolt;
}
Expand All @@ -258,17 +261,18 @@ float pmMeasureExtBatteryCurrent(void)
return current;
}

void pmEnableExtBatteryVoltMeasuring(uint8_t pin, float multiplier)
void pmEnableExtBatteryVoltMeasuring(const deckPin_t pin, float multiplier)
{
extBatVoltDeckPin = pin;
isExtBatVoltDeckPinSet = true;
extBatVoltMultiplier = multiplier;
}

float pmMeasureExtBatteryVoltage(void)
{
float voltage;

if (extBatVoltDeckPin)
if (isExtBatVoltDeckPinSet)
{
voltage = analogReadVoltage(extBatVoltDeckPin) * extBatVoltMultiplier;
}
Expand Down

0 comments on commit b8ba12c

Please sign in to comment.