Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • authierj/dfall-system
1 result
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 4798 deletions
crazyflie-firmware-src/src/deck/drivers/interface/locodeck.h deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2016 Bitcraze AB
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* locodeck.h: Dwm1000 deck driver.
*/
#ifndef __LOCODECK_H__
#define __LOCODECK_H__
#include "libdw1000.h"
#include "stabilizer_types.h"
#define SPEED_OF_LIGHT 299792458.0
// Timestamp counter frequency
#define LOCODECK_TS_FREQ (499.2e6 * 128)
typedef enum uwbEvent_e {
eventTimeout,
eventPacketReceived,
eventPacketSent,
eventReceiveTimeout,
eventReceiveFailed,
} uwbEvent_t;
#define LOCODECK_NR_OF_ANCHORS 6
typedef uint64_t locoAddress_t;
typedef struct {
const uint64_t antennaDelay;
const int rangingFailedThreshold;
const locoAddress_t tagAddress;
const locoAddress_t anchorAddress[LOCODECK_NR_OF_ANCHORS];
point_t anchorPosition[LOCODECK_NR_OF_ANCHORS];
bool anchorPositionOk;
float distance[LOCODECK_NR_OF_ANCHORS];
float pressures[LOCODECK_NR_OF_ANCHORS];
int failedRanging[LOCODECK_NR_OF_ANCHORS];
volatile uint16_t rangingState;
} lpsAlgoOptions_t;
// Callback for one uwb algorithm
typedef struct uwbAlgorithm_s {
void (*init)(dwDevice_t *dev, lpsAlgoOptions_t* options);
uint32_t (*onEvent)(dwDevice_t *dev, uwbEvent_t event);
} uwbAlgorithm_t;
#include <FreeRTOS.h>
#define MAX_TIMEOUT portMAX_DELAY
#endif // __LOCODECK_H__
crazyflie-firmware-src/src/deck/drivers/interface/lpsTdoaTag.h deleted 100755 → 0
View file @ 6291d058
#ifndef __LPS_TDOA_TAG_H__
#define __LPS_TDOA_TAG_H__
#include "locodeck.h"
#include "libdw1000.h"
#include "mac.h"
extern uwbAlgorithm_t uwbTdoaTagAlgorithm;
typedef struct rangePacket_s {
uint8_t type;
uint8_t txMaster[5];
uint8_t timestamps[LOCODECK_NR_OF_ANCHORS][5];
} __attribute__((packed)) rangePacket_t;
#endif // __LPS_TDOA_TAG_H__
crazyflie-firmware-src/src/deck/drivers/interface/lpsTwrTag.h deleted 100755 → 0
View file @ 6291d058
#ifndef __LPS_TWR_TAG_H__
#define __LPS_TWR_TAG_H__
#include "locodeck.h"
#include "libdw1000.h"
#include "mac.h"
#define LPS_TWR_POLL 0x01 // Poll is initiated by the tag
#define LPS_TWR_ANSWER 0x02
#define LPS_TWR_FINAL 0x03
#define LPS_TWR_REPORT 0x04 // Report contains all measurement from the anchor
#define LPS_TWR_TYPE 0
#define LPS_TWR_SEQ 1
extern uwbAlgorithm_t uwbTwrTagAlgorithm;
typedef struct {
uint8_t pollRx[5];
uint8_t answerTx[5];
uint8_t finalRx[5];
float pressure;
float temperature;
float asl;
uint8_t pressure_ok;
} __attribute__((packed)) lpsTwrTagReportPayload_t;
#endif // __LPS_TWR_TAG_H__
\ No newline at end of file
crazyflie-firmware-src/src/deck/drivers/interface/mac.h deleted 100755 → 0
View file @ 6291d058
#ifndef __MAC_H__
#define __MAC_H__
#include <stdint.h>
#include "locodeck.h"
// Packet format with compressed PAN and 64Bit addresses
// Maximum 64 bytes payload
typedef struct packet_s {
union {
uint16_t fcf;
struct {
uint16_t type:3;
uint16_t security:1;
uint16_t framePending:1;
uint16_t ack:1;
uint16_t ipan:1;
uint16_t reserved:3;
uint16_t destAddrMode:2;
uint16_t version:2;
uint16_t srcAddrMode:2;
} fcf_s;
};
uint8_t seq;
uint16_t pan;
locoAddress_t destAddress;
locoAddress_t sourceAddress;
uint8_t payload[64];
} __attribute__((packed)) packet_t;
#define MAC80215_PACKET_INIT(packet, TYPE) packet.fcf_s.type = (TYPE); \
packet.fcf_s.security = 0; \
packet.fcf_s.framePending = 0; \
packet.fcf_s.ack = 0; \
packet.fcf_s.ipan = 1; \
packet.fcf_s.destAddrMode = 3; \
packet.fcf_s.version = 1; \
packet.fcf_s.srcAddrMode = 3;
#define MAC802154_TYPE_BEACON 0
#define MAC802154_TYPE_DATA 1
#define MAC802154_TYPE_ACK 2
#define MAC802154_TYPE_CMD 3
#define MAC802154_HEADER_LENGTH 21
#endif
crazyflie-firmware-src/src/deck/drivers/interface/usddeck.h deleted 100755 → 0
View file @ 6291d058
#ifndef __USBDECK_H__
#define __USBDECK_H__
#include <stdint.h>
#include <stdbool.h>
#endif //__USBDECK_H__
crazyflie-firmware-src/src/deck/drivers/interface/vl53l0x.h deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* 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, in version 3.
*
* 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/>.
*
* vl53l0x.h: Time-of-flight distance sensor driver
*/
#ifndef _VL53L0X_H_
#define _VL53L0X_H_
#define VL53L0X_DEFAULT_ADDRESS 0b0101001
#define VL53L0X_RA_SYSRANGE_START 0x00
#define VL53L0X_RA_SYSTEM_THRESH_HIGH 0x0C
#define VL53L0X_RA_SYSTEM_THRESH_LOW 0x0E
#define VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG 0x01
#define VL53L0X_RA_SYSTEM_RANGE_CONFIG 0x09
#define VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD 0x04
#define VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO 0x0A
#define VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH 0x84
#define VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR 0x0B
#define VL53L0X_RA_RESULT_INTERRUPT_STATUS 0x13
#define VL53L0X_RA_RESULT_RANGE_STATUS 0x14
#define VL53L0X_RA_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0xBC
#define VL53L0X_RA_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0xC0
#define VL53L0X_RA_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0xD0
#define VL53L0X_RA_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0xD4
#define VL53L0X_RA_RESULT_PEAK_SIGNAL_RATE_REF 0xB6
#define VL53L0X_RA_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x28
#define VL53L0X_RA_I2C_SLAVE_DEVICE_ADDRESS 0x8A
#define VL53L0X_RA_MSRC_CONFIG_CONTROL 0x60
#define VL53L0X_RA_PRE_RANGE_CONFIG_MIN_SNR 0x27
#define VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x56
#define VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x57
#define VL53L0X_RA_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x64
#define VL53L0X_RA_FINAL_RANGE_CONFIG_MIN_SNR 0x67
#define VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x47
#define VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x48
#define VL53L0X_RA_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x44
#define VL53L0X_RA_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0x61
#define VL53L0X_RA_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0x62
#define VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50
#define VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x51
#define VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x52
#define VL53L0X_RA_SYSTEM_HISTOGRAM_BIN 0x81
#define VL53L0X_RA_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x33
#define VL53L0X_RA_HISTOGRAM_CONFIG_READOUT_CTRL 0x55
#define VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x71
#define VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x72
#define VL53L0X_RA_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x20
#define VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP 0x46
#define VL53L0X_RA_SOFT_RESET_GO2_SOFT_RESET_N 0xBF
#define VL53L0X_RA_IDENTIFICATION_MODEL_ID 0xC0
#define VL53L0X_RA_IDENTIFICATION_REVISION_ID 0xC2
#define VL53L0X_IDENTIFICATION_MODEL_ID 0xEEAA
#define VL53L0X_IDENTIFICATION_REVISION_ID 0x10
#define VL53L0X_RA_OSC_CALIBRATE_VAL 0xF8
#define VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH 0x32
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0xB0
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0xB1
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0xB2
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0xB3
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0xB4
#define VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0xB5
#define VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
#define VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E
#define VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F
#define VL53L0X_RA_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
#define VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x89
#define VL53L0X_RA_ALGO_PHASECAL_LIM 0x30
#define VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT 0x30
// TCC: Target CentreCheck
// MSRC: Minimum Signal Rate Check
// DSS: Dynamic Spad Selection
typedef struct {
bool tcc;
bool msrc;
bool dss;
bool pre_range;
bool final_range;
} SequenceStepEnables;
typedef struct
{
uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks;
uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks;
uint32_t msrc_dss_tcc_us, pre_range_us, final_range_us;
} SequenceStepTimeouts;
typedef enum vcselPeriodType_t { VcselPeriodPreRange, VcselPeriodFinalRange } vcselPeriodType;
/** Default constructor, uses external I2C address.
* @see VL53L0X_DEFAULT_ADDRESS
*/
void vl53l0xInit(DeckInfo* info);
bool vl53l0xTest(void);
void vl53l0xTask(void* arg);
/** Verify the I2C connection.
* Make sure the device is connected and responds as expected.
* @return True if connection is valid, false otherwise
*/
bool vl53l0xTestConnection();
/** Get Model ID.
* This register is used to verify the model number of the device,
* but only before it has been configured to run
* @return Model ID
* @see VL53L0X_RA_IDENTIFICATION_MODEL_ID
* @see VL53L0X_IDENTIFICATION_MODEL_ID
*/
uint16_t vl53l0xGetModelID();
/** Get Revision ID.
* This register is used to verify the revision number of the device,
* but only before it has been configured to run
* @return Revision ID
* @see VL53L0X_RA_IDENTIFICATION_REVISION_ID
* @see VL53L0X_IDENTIFICATION_REVISION_ID
*/
uint8_t vl53l0xGetRevisionID();
// Initialize sensor using sequence based on VL53L0X_DataInit(),
// VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration().
// This function does not perform reference SPAD calibration
// (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it
// is performed by ST on the bare modules; it seems like that should work well
// enough unless a cover glass is added.
// If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
// mode.
bool vl53l0xInitSensor(bool io_2v8);
// Set the return signal rate limit check value in units of MCPS (mega counts
// per second). "This represents the amplitude of the signal reflected from the
// target and detected by the device"; setting this limit presumably determines
// the minimum measurement necessary for the sensor to report a valid reading.
// Setting a lower limit increases the potential range of the sensor but also
// seems to increase the likelihood of getting an inaccurate reading because of
// unwanted reflections from objects other than the intended target.
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool vl53l0xSetSignalRateLimit(float limit_Mcps);
// Set the measurement timing budget in microseconds, which is the time allowed
// for one measurement; the ST API and this library take care of splitting the
// timing budget among the sub-steps in the ranging sequence. A longer timing
// budget allows for more accurate measurements. Increasing the budget by a
// factor of N decreases the range measurement standard deviation by a factor of
// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
bool vl53l0xSetMeasurementTimingBudget(uint32_t budget_us);
// Get the measurement timing budget in microseconds
// based on VL53L0X_get_measurement_timing_budget_micro_seconds()
// in us
uint32_t vl53l0xGetMeasurementTimingBudget(void);
// Set the VCSEL (vertical cavity surface emitting laser) pulse period for the
// given period type (pre-range or final range) to the given value in PCLKs.
// Longer periods seem to increase the potential range of the sensor.
// Valid values are (even numbers only):
// pre: 12 to 18 (initialized default: 14)
// final: 8 to 14 (initialized default: 10)
// based on VL53L0X_set_vcsel_pulse_period()
bool vl53l0xSetVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks);
// Get the VCSEL pulse period in PCLKs for the given period type.
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t vl53l0xGetVcselPulsePeriod(vcselPeriodType type);
// Start continuous ranging measurements. If period_ms (optional) is 0 or not
// given, continuous back-to-back mode is used (the sensor takes measurements as
// often as possible); otherwise, continuous timed mode is used, with the given
// inter-measurement period in milliseconds determining how often the sensor
// takes a measurement.
// based on VL53L0X_StartMeasurement()
void vl53l0xStartContinuous(uint32_t period_ms);
// Stop continuous measurements
// based on VL53L0X_StopMeasurement()
void vl53l0xStopContinuous(void);
// Returns a range reading in millimeters when continuous mode is active
// (readRangeSingleMillimeters() also calls this function after starting a
// single-shot range measurement)
uint16_t vl53l0xReadRangeContinuousMillimeters(void);
// Performs a single-shot range measurement and returns the reading in
// millimeters
// based on VL53L0X_PerformSingleRangingMeasurement()
uint16_t vl53l0xReadRangeSingleMillimeters(void);
#endif /* _VL53L0X_H_ */
crazyflie-firmware-src/src/deck/drivers/src/bigquad.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* exptest.c - Testing of expansion port.
*/
#define DEBUG_MODULE "BIGQUAD"
#include <stdint.h>
#include <string.h>
#include "stm32fxxx.h"
#include "config.h"
#include "motors.h"
#include "debug.h"
#include "deck.h"
#include "extrx.h"
#include "pm.h"
#define BIGQUAD_BAT_VOLT_PIN DECK_GPIO_MISO
#define BIGQUAD_BAT_VOLT_MULT 7.8f
#define BIGQUAD_BAT_CURR_PIN DECK_GPIO_SCK
#define BIGQUAD_BAT_AMP_PER_VOLT 1.0f
//Hardware configuration
static bool isInit;
static void bigquadInit(DeckInfo *info)
{
if(isInit)
return;
DEBUG_PRINT("Switching to brushless.\n");
motorsInit(motorMapBigQuadDeck);
extRxInit();
#ifdef BQ_DECK_ENABLE_PM
pmEnableExtBatteryVoltMeasuring(BIGQUAD_BAT_VOLT_PIN, BIGQUAD_BAT_VOLT_MULT);
pmEnableExtBatteryCurrMeasuring(BIGQUAD_BAT_CURR_PIN, BIGQUAD_BAT_AMP_PER_VOLT);
#endif
isInit = true;
}
static bool bigquadTest()
{
bool status = true;
if(!isInit)
return false;
status = motorsTest();
return status;
}
static const DeckDriver bigquad_deck = {
.vid = 0xBC,
.pid = 0x05,
.name = "bcBigQuad",
.usedPeriph = DECK_USING_TIMER3 | DECK_USING_TIMER14,
.usedGpio = DECK_USING_PA2 | DECK_USING_PA3 | DECK_USING_PB4 | DECK_USING_PB5 | DECK_USING_PA7,
.init = bigquadInit,
.test = bigquadTest,
};
#ifdef ENABLE_BQ_DECK
DECK_DRIVER(bigquad_deck);
#endif
crazyflie-firmware-src/src/deck/drivers/src/buzzdeck.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2015 BitCraze AB
*
* 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, in version 3.
*
* 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/>.
*
* buzzdeck.c - Deck driver for the Crazyflie 2.0 buzzer deck
*/
#include <stdint.h>
#include <stdlib.h>
#include "stm32fxxx.h"
#include "deck.h"
#include "buzzer.h"
#include "piezo.h"
static void buzzDeckOn(uint32_t freq)
{
piezoSetRatio(128);
piezoSetFreq(freq);
}
static void buzzDeckOff()
{
piezoSetRatio(0);
}
static struct buzzerControl buzzDeckCtrl = {
.on = buzzDeckOn,
.off = buzzDeckOff
};
static void buzzDeckInit(DeckInfo *info)
{
piezoInit();
buzzerSetControl(&buzzDeckCtrl);
}
static const DeckDriver buzzer_deck = {
.vid = 0xBC,
.pid = 0x04,
.name = "bcBuzzer",
.usedPeriph = DECK_USING_TIMER5,
.usedGpio = DECK_USING_TX2 | DECK_USING_RX2,
.init = buzzDeckInit,
};
DECK_DRIVER(buzzer_deck);
crazyflie-firmware-src/src/deck/drivers/src/cppmdeck.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* exptest.c - Testing of expansion port.
*/
#define DEBUG_MODULE "CPPM"
#include <stdint.h>
#include <string.h>
#include "stm32fxxx.h"
#include "config.h"
#include "debug.h"
#include "deck.h"
#include "extrx.h"
//Hardware configuration
static bool isInit;
static void cppmdeckInit(DeckInfo *info)
{
if(isInit)
return;
extRxInit();
isInit = true;
}
static bool cppmdeckTest()
{
bool status = true;
if(!isInit)
return false;
return status;
}
static const DeckDriver cppm_deck = {
.vid = 0,
.pid = 0,
.name = "bcCPPM",
.usedPeriph = DECK_USING_TIMER14,
.usedGpio = DECK_USING_PA7,
.init = cppmdeckInit,
.test = cppmdeckTest,
};
DECK_DRIVER(cppm_deck);
crazyflie-firmware-src/src/deck/drivers/src/gtgps.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* exptest.c - Testing of expansion port.
*/
#define DEBUG_MODULE "GTGPS"
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include "stm32fxxx.h"
#include "config.h"
#include "console.h"
#include "uart1.h"
#include "debug.h"
#include "deck.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "log.h"
static bool isInit;
#define LEN_TOKEN 5
#define MAX_LEN_SENTANCE 100
char buff[MAX_LEN_SENTANCE];
uint8_t bi;
typedef bool (*SentanceParser)(char * buff);
typedef struct {
const char * token;
SentanceParser parser;
} ParserConfig;
typedef enum {
FixNone = 1,
Fix2D = 2,
Fix3D = 3
} FixQuality;
typedef enum {
NoFix = 1,
GPSFix = 2
} FixType;
typedef enum {FIELD_COORD, FIELD_FLOAT, FIELD_INT} FieldType;
typedef struct {
FixQuality fix;
uint32_t locks[12];
float pdop;
float hdop;
float vdop;
} Basic;
typedef struct {
uint32_t fixtime;
int32_t latitude;
int32_t longitude;
FixType fixtype;
uint32_t nsat;
float hdop;
float alt;
float height;
} MeasData;
static Basic b;
static MeasData m;
// Only use on 0-terminated strings!
static int skip_to_next(char ** sp, const char ch) {
int steps;
while (ch != 0 && (**sp) != ch) {
(*sp)++;
steps++;
}
if (ch != 0)
(*sp)++;
return (ch != 0 ? steps : -1);
}
static int32_t parse_coordinate(char ** sp) {
int32_t dm;
int32_t degree;
int32_t minute;
int32_t second;
int32_t ret;
char * i;
char * j;
// Format as DDDMM.SSSS converted by long or lat = DDD + MM / 100 + SSSS/3600
// To avoid inaccuracy caused by float representation save this value as
// a large number * 10 M
// 32 18.0489 N = 32 degrees + 18.0489 / 60 = 32.300815 N
dm = strtol(*sp, &i, 10);
degree = (dm / 100) * 10000000;
minute = ((dm % 100) * 10000000) / 60;
second = (strtol(i+1, &j, 10) * 1000) / 60;
ret = degree + minute + second;
skip_to_next(sp, ',');
if (**sp == 'S' || **sp == 'W')
ret *= -1;
return ret;
}
static float parse_float(char * sp) {
float ret = 0;
int major = 0;
int minor = 0;
int deci_nbr = 0;
char * i;
char * j;
major = strtol(sp, &i, 10);
// Do decimals
if (strncmp(i, ".", 1) == 0) {
minor = strtol(i+1, &j, 10);
deci_nbr = j - i - 1;
}
ret = (major * pow(10, deci_nbr) + minor) / pow(10, deci_nbr);
//printf("%i.%i == %f (%i) (%c)\n", major, minor, ret, deci_nbr, (int) *i);
return ret;
}
static void parse_next(char ** sp, FieldType t, void * value) {
skip_to_next(sp, ',');
//DEBUG_PRINT("[%s]\n", (*sp));
switch (t) {
case FIELD_INT:
*((uint32_t*) value) = strtol(*sp, 0, 10);
break;
case FIELD_FLOAT:
*((float*) value) = parse_float(*sp);
break;
case FIELD_COORD:
*((int32_t*) value) = parse_coordinate(sp);
}
}
static bool gpgsaParser(char * buff) {
int i = 0;
char * sp = buff;
// Skip leading A/M
skip_to_next(&sp, ',');
parse_next(&sp, FIELD_INT, &b.fix);
for (i = 0; i < 12; i++) {
parse_next(&sp, FIELD_INT, &b.locks[i]);
}
parse_next(&sp, FIELD_FLOAT, &b.pdop);
parse_next(&sp, FIELD_FLOAT, &b.hdop);
parse_next(&sp, FIELD_FLOAT, &b.vdop);
//dbg_print_basic(&b);
return false;
}
static bool gpggaParser(char * buff) {
char * sp = buff;
parse_next(&sp, FIELD_INT, &m.fixtime);
parse_next(&sp, FIELD_COORD, &m.latitude);
parse_next(&sp, FIELD_COORD, &m.longitude);
parse_next(&sp, FIELD_INT, &m.fixtype);
parse_next(&sp, FIELD_INT, &m.nsat);
parse_next(&sp, FIELD_FLOAT, &m.hdop);
parse_next(&sp, FIELD_FLOAT, &m.alt);
skip_to_next(&sp, ',');
// Unit for altitude (not used yet)
parse_next(&sp, FIELD_FLOAT, &m.height);
skip_to_next(&sp, ',');
// Unit for height (not used yet)
skip_to_next(&sp, ',');
//consolePutchar('.');
//consoleFlush();
return false;
}
static ParserConfig parsers[] = {
{.token = "GPGSA", .parser = gpgsaParser},
{.token = "GPGGA", .parser = gpggaParser}
};
static bool verifyChecksum(const char * buff) {
uint8_t test_chksum = 0;
uint32_t ref_chksum = 0;
uint8_t i = 0;
while (buff[i] != '*' && i < MAX_LEN_SENTANCE-3) {
test_chksum ^= buff[i++];
}
ref_chksum = strtol(&buff[i+1], 0, 16);
return (test_chksum == ref_chksum);
}
static uint8_t baudcmd[] = "$PMTK251,115200*1F\r\n";
// 5 Hz
static uint8_t updaterate[] = "$PMTK220,200*2C\r\n";
static uint8_t updaterate2[] = "$PMTK300,200,0,0,0,0*2F\r\n";
// 10 Hz
//static uint8_t updaterate3[] = "$PMTK220,100*2F\r\n";
//static uint8_t updaterate4[] = "$PMTK300,100,0,0,0,0*2C\r\n";
void gtgpsTask(void *param)
{
char ch;
int j;
uart1SendData(sizeof(baudcmd), baudcmd);
vTaskDelay(500);
uart1Init(115200);
vTaskDelay(500);
uart1SendData(sizeof(updaterate), updaterate);
uart1SendData(sizeof(updaterate2), updaterate2);
// uart1SendData(sizeof(updaterate3), updaterate3);
// uart1SendData(sizeof(updaterate4), updaterate4);
while(1)
{
uart1Getchar(&ch);
consolePutchar(ch);
if (ch == '$') {
bi = 0;
} else if (ch == '\n') {
buff[bi] = 0; // Terminate with null
if (verifyChecksum(buff)) {
//DEBUG_PRINT("O");
for (j = 0; j < sizeof(parsers)/sizeof(parsers[0]); j++) {
if (strncmp(parsers[j].token, buff, LEN_TOKEN) == 0) {
parsers[j].parser(&buff[LEN_TOKEN]);
}
}
}
} else if (bi < MAX_LEN_SENTANCE) {
buff[bi++] = ch;
}
}
}
static void gtgpsInit(DeckInfo *info)
{
if(isInit)
return;
DEBUG_PRINT("Enabling reading from GlobalTop GPS\n");
uart1Init(9600);
xTaskCreate(gtgpsTask, "GTGPS",
configMINIMAL_STACK_SIZE, NULL, /*priority*/1, NULL);
isInit = true;
}
static bool gtgpsTest()
{
bool status = true;
if(!isInit)
return false;
return status;
}
static const DeckDriver gtgps_deck = {
.vid = 0xBC,
.pid = 0x07,
.name = "bcGTGPS",
.usedPeriph = 0,
.usedGpio = 0, // FIXME: Edit the used GPIOs
.init = gtgpsInit,
.test = gtgpsTest,
};
DECK_DRIVER(gtgps_deck);
LOG_GROUP_START(gps)
LOG_ADD(LOG_INT32, lat, &m.latitude)
LOG_ADD(LOG_INT32, lon, &m.longitude)
LOG_ADD(LOG_FLOAT, hMSL, &m.height)
LOG_ADD(LOG_FLOAT, hAcc, &b.pdop)
LOG_ADD(LOG_INT32, nsat, &m.nsat)
LOG_ADD(LOG_INT32, fix, &b.fix)
LOG_GROUP_STOP(gps)
crazyflie-firmware-src/src/deck/drivers/src/ledring12.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* 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, in version 3.
*
* 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/>.
*
* ledring12.c: RGB Ring 12 Leds effects/driver
*/
#include "ledring12.h"
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "stm32fxxx.h"
#include "deck.h"
#include "FreeRTOS.h"
#include "timers.h"
#include "ledring12.h"
#include "ws2812.h"
#include "worker.h"
#include "param.h"
#include "pm.h"
#include "log.h"
/*
* To add a new effect just add it as a static function with the prototype
* void effect(uint8_t buffer[][3], bool reset)
*
* Then add it to the effectsFct[] list bellow. It will automatically be
* activated using the ring.effect parameter.
*
* The ring color needs to be written in the buffer argument. The buffer is not
* modified in memory as long as reset is not 'true', see the spin effects for
* and example.
*
* The log subsystem can be used to get the value of any log variable of the
* system. See tiltEffect for an example.
*/
typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset);
/**************** Some useful macros ***************/
#define RED {0x10, 0x00, 0x00}
#define GREEN {0x00, 0x10, 0x00}
#define BLUE {0x00, 0x00, 0x10}
#define WHITE {0xff, 0xff, 0xff}
#define BLACK {0x00, 0x00, 0x00}
#define MAX(a,b) ((a>b)?a:b)
#define MIN(a,b) ((a<b)?a:b)
#define COPY_COLOR(dest, orig) dest[0]=orig[0]; dest[1]=orig[1]; dest[2]=orig[2]
#define ADD_COLOR(dest, o1, o2) dest[0]=(o1[0]>>1)+(o2[0]>>1);dest[1]=(o1[1]>>1)+(o2[1]>>1);dest[2]=(o1[2]>>1)+(o2[2]>>1);
#define LIMIT(a) ((a>255)?255:(a<0)?0:a)
#define SIGN(a) ((a>=0)?1:-1)
#define DEADBAND(a, b) ((a<b) ? 0:a)
#define LINSCALE(domain_low, domain_high, codomain_low, codomain_high, value) ((codomain_high - codomain_low) / (domain_high - domain_low)) * (value - domain_low) + codomain_low
#define SET_WHITE(dest, intensity) dest[0] = intensity; dest[1] = intensity; dest[2] = intensity;
static uint32_t effect = 6;
static uint32_t neffect;
static uint8_t headlightEnable = 0;
static uint8_t black[][3] = {BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
};
static const uint8_t green[] = {0x00, 0xFF, 0x00};
static const uint8_t red[] = {0xFF, 0x00, 0x00};
static const uint8_t blue[] = {0x00, 0x00, 0xFF};
static const uint8_t white[] = WHITE;
static const uint8_t part_black[] = BLACK;
uint8_t ledringmem[NBR_LEDS * 2];
/**************** Black (LEDs OFF) ***************/
static void blackEffect(uint8_t buffer[][3], bool reset)
{
int i;
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
buffer[i][0] = 0;
buffer[i][1] = 0;
buffer[i][2] = 0;
}
}
}
/**************** White spin ***************/
static const uint8_t whiteRing[][3] = {{32, 32, 32}, {8,8,8}, {2,2,2},
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
};
static const uint8_t blueRing[][3] = {{64, 64, 255}, {32,32,64}, {8,8,16},
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
};
static const uint8_t greenRing[][3] = {{64, 255, 64}, {32,64,32}, {8,16,8},
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
};
static const uint8_t redRing[][3] = {{64, 0, 0}, {16,0,0}, {8,0,0},
{4,0,0}, {2,0,0}, {1,0,0},
BLACK, BLACK, BLACK,
BLACK, BLACK, BLACK,
};
static void whiteSpinEffect(uint8_t buffer[][3], bool reset)
{
int i;
uint8_t temp[3];
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], whiteRing[i]);
}
}
COPY_COLOR(temp, buffer[0]);
for (i=0; i<(NBR_LEDS-1); i++) {
COPY_COLOR(buffer[i], buffer[i+1]);
}
COPY_COLOR(buffer[(NBR_LEDS-1)], temp);
}
static uint8_t solidRed=20, solidGreen=20, solidBlue=20;
static float glowstep = 0.05;
static void solidColorEffect(uint8_t buffer[][3], bool reset)
{
int i;
static float brightness=0;
if (reset) brightness = 0;
if (brightness<1) brightness += 0.05f;
else brightness = 1;
for (i=0; i<NBR_LEDS; i++)
{
buffer[i][0] = solidRed*brightness;
buffer[i][1] = solidGreen*brightness;
buffer[i][2] = solidBlue*brightness;
}
}
static void virtualMemEffect(uint8_t buffer[][3], bool reset)
{
int i;
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], part_black);
}
}
for (i = 0; i < NBR_LEDS; i++)
{
uint8_t R5, G6, B5;
uint8_t (*led)[2] = (uint8_t (*)[2])ledringmem;
// Convert from RGB565 to RGB888
R5 = led[i][0] >> 3;
G6 = ((led[i][0] & 0x07) << 3) | (led[i][1] >> 5);
B5 = led[i][1] & 0x1F;
buffer[i][0] = ((uint16_t)R5 * 527 + 23 ) >> 6;
buffer[i][1] = ((uint16_t)G6 * 259 + 33 ) >> 6;
buffer[i][2] = ((uint16_t)B5 * 527 + 23 ) >> 6;
}
}
static void boatEffect(uint8_t buffer[][3], bool reset)
{
int i;
uint8_t reds[] = {1,2,3,4,5};
uint8_t greens[] = {7,8,9,10,11};
uint8_t whites[] = {0};
uint8_t blacks[] = {6};
for (i=0; i<sizeof(reds); i++)
{
COPY_COLOR(buffer[reds[i]], red);
}
for (i=0; i<sizeof(greens); i++)
{
COPY_COLOR(buffer[greens[i]], green);
}
for (i=0; i<sizeof(whites); i++)
{
COPY_COLOR(buffer[whites[i]], white);
}
for (i=0; i<sizeof(blacks); i++)
{
COPY_COLOR(buffer[blacks[i]], part_black);
}
}
/**************** Color spin ***************/
static const uint8_t colorRing[][3] = {{0,0,32}, {0,0,16}, {0,0,8},
{0,0,4}, {16,16,16}, {8,8,8},
{4,4,4},{32,0,0},{16,0,0},
{8,0,0}, {4,0,0}, {2,0,0},
};
static void colorSpinEffect(uint8_t buffer[][3], bool reset)
{
int i;
uint8_t temp[3];
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], colorRing[i]);
}
}
COPY_COLOR(temp, buffer[0]);
for (i=0; i<(NBR_LEDS-1); i++) {
COPY_COLOR(buffer[i], buffer[i+1]);
}
COPY_COLOR(buffer[(NBR_LEDS-1)], temp);
}
static void spinEffect2(uint8_t buffer[][3], bool reset)
{
int i;
uint8_t temp[3];
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[(NBR_LEDS-i)%NBR_LEDS], blueRing[i]);
}
}
COPY_COLOR(temp, buffer[(NBR_LEDS-1)]);
for (i=(NBR_LEDS-1); i>=0; i--) {
COPY_COLOR(buffer[i], buffer[i-1]);
}
COPY_COLOR(buffer[0], temp);
}
static void doubleSpinEffect(uint8_t buffer[][3], bool reset) {
static uint8_t sub1[NBR_LEDS][3];
static uint8_t sub2[NBR_LEDS][3];
int i;
static int step;
if (reset) step = 0;
whiteSpinEffect(sub1, reset);
spinEffect2(sub2, reset);
//if ((step%3)) spinEffect2(sub2, false);
//if (reset) spinEffect2(sub2, true);
for (i=0; i<NBR_LEDS; i++)
{
ADD_COLOR(buffer[i], sub1[i], sub2[i]);
}
step ++;
}
/**************** Dynamic tilt effect ***************/
static void tiltEffect(uint8_t buffer[][3], bool reset)
{
static int pitchid, rollid, thrust=-1;
// 2014-12-28 chad: Reset LEDs to off to avoid color artifacts
// when switching from other effects.
if (reset)
{
int i;
for (i=0; i<NBR_LEDS; i++) {
buffer[i][0] = 0;
buffer[i][1] = 0;
buffer[i][2] = 0;
}
}
if (thrust<0) {
//Init
pitchid = logGetVarId("stabilizer", "pitch");
rollid = logGetVarId("stabilizer", "roll");
thrust = logGetVarId("stabilizer", "thrust");
} else {
const int led_middle = 10;
float pitch = -1*logGetFloat(pitchid);
float roll = -1*logGetFloat(rollid);
pitch = (pitch>20)?20:(pitch<-20)?-20:pitch;
roll = (roll>20)?20:(roll<-20)?-20:roll;
pitch=SIGN(pitch)*pitch*pitch;
roll*=SIGN(roll)*roll;
buffer[11][0] = LIMIT(led_middle + pitch);
buffer[0][0] = LIMIT(led_middle + pitch);
buffer[1][0] = LIMIT(led_middle + pitch);
buffer[2][2] = LIMIT(led_middle - roll);
buffer[3][2] = LIMIT(led_middle - roll);
buffer[4][2] = LIMIT(led_middle - roll);
buffer[5][0] = LIMIT(led_middle - pitch);
buffer[6][0] = LIMIT(led_middle - pitch);
buffer[7][0] = LIMIT(led_middle - pitch);
buffer[8][2] = LIMIT(led_middle + roll);
buffer[9][2] = LIMIT(led_middle + roll);
buffer[10][2] = LIMIT(led_middle + roll);
}
}
/*************** Gravity light effect *******************/
static float gravityLightCalculateAngle(float pitch, float roll);
static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensity);
static void gravityLight(uint8_t buffer[][3], bool reset)
{
static int pitchid, rollid;
static bool isInitialized = false;
if (!isInitialized) {
pitchid = logGetVarId("stabilizer", "pitch");
rollid = logGetVarId("stabilizer", "roll");
isInitialized = true;
}
float pitch = logGetFloat(pitchid); // -180 to 180
float roll = logGetFloat(rollid); // -180 to 180
float angle = gravityLightCalculateAngle(pitch, roll);
float led_index = NBR_LEDS * angle / (2 * (float) M_PI);
int intensity = LIMIT(sqrtf(pitch * pitch + roll * roll));
gravityLightRender(buffer, led_index, intensity);
}
static float gravityLightCalculateAngle(float pitch, float roll) {
float angle = 0.0;
if (roll != 0) {
angle = atanf(pitch / roll) + (float) M_PI_2;
if (roll < 0.0) {
angle += (float) M_PI;
}
}
return angle;
}
static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensity) {
float width = 5;
float height = intensity;
int i;
for (i = 0; i < NBR_LEDS; i++) {
float distance = fabsf(led_index - i);
if (distance > NBR_LEDS / 2) {
distance = NBR_LEDS - distance;
}
int col = height - distance * (height / (width / 2));
SET_WHITE(buffer[i], LIMIT(col));
}
}
/*************** Brightness effect ********************/
#define MAX_RATE 512
static void brightnessEffect(uint8_t buffer[][3], bool reset)
{
static int gyroYid, gyroZid, gyroXid =- 1;
static uint8_t brightness = 0;
if (gyroXid < 0)
{
//Init
gyroXid = logGetVarId("gyro", "x");
gyroYid = logGetVarId("gyro", "y");
gyroZid = logGetVarId("gyro", "z");
}
else
{
int i;
int gyroX = (int)logGetFloat(gyroXid);
int gyroY = (int)logGetFloat(gyroYid);
int gyroZ = (int)logGetFloat(gyroZid);
// Adjust to interval
gyroX = (gyroX>MAX_RATE) ? MAX_RATE:(gyroX<-MAX_RATE) ? -MAX_RATE:gyroX;
gyroY = (gyroY>MAX_RATE) ? MAX_RATE:(gyroY<-MAX_RATE) ? -MAX_RATE:gyroY;
gyroZ = (gyroZ>MAX_RATE) ? MAX_RATE:(gyroZ<-MAX_RATE) ? -MAX_RATE:gyroZ;
gyroX = SIGN(gyroX) * gyroX / 2;
gyroY = SIGN(gyroY) * gyroY / 2;
gyroZ = SIGN(gyroZ) * gyroZ / 2;
gyroX = DEADBAND(gyroX, 5);
gyroY = DEADBAND(gyroY, 5);
gyroZ = DEADBAND(gyroZ, 5);
for (i=0; i < NBR_LEDS; i++)
{
buffer[i][0] = (uint8_t)(LIMIT(gyroZ));
buffer[i][1] = (uint8_t)(LIMIT(gyroY));
buffer[i][2] = (uint8_t)(LIMIT(gyroX));
}
brightness++;
}
}
static void setHeadlightsOn(bool on)
{
if (on)
GPIO_SetBits(GPIOB, GPIO_Pin_4);
else
GPIO_ResetBits(GPIOB, GPIO_Pin_4);
}
/* LED-ring test effect */
#define TEST_INTENTS 20
static uint8_t test_pat[3][3] = {{TEST_INTENTS, 0, 0}, {0, TEST_INTENTS, 0}, {0, 0, TEST_INTENTS}};
static uint8_t test_eff_nbr = 0;
#define TEST_DELAY 4
static uint8_t test_delay_counter = 0;
static uint8_t headlight_test_counter =0;
static uint8_t test_front = false;
static void ledTestEffect(uint8_t buffer[][3], bool reset)
{
int i;
static float brightness=0;
if (reset) brightness = 0;
if (brightness<1) brightness += 0.05f;
else brightness = 1;
for (i=0; i<NBR_LEDS; i++)
{
buffer[i][0] = test_pat[test_eff_nbr][0];
buffer[i][1] = test_pat[test_eff_nbr][1];
buffer[i][2] = test_pat[test_eff_nbr][2];
}
test_delay_counter++;
headlight_test_counter++;
if (test_delay_counter > TEST_DELAY) {
test_delay_counter = 0;
test_eff_nbr = (test_eff_nbr + 1) % 3;
}
if (headlight_test_counter > (TEST_DELAY*3)) {
headlight_test_counter = 0;
test_front = !test_front;
headlightEnable = test_front;
}
}
/**
* An effect that shows the battery charge on the LED ring.
*
* Red means empty, blue means full.
*/
static float emptyCharge = 3.1, fullCharge = 4.2;
static void batteryChargeEffect(uint8_t buffer[][3], bool reset)
{
int i;
static int vbatid;
float vbat;
vbatid = logGetVarId("pm", "vbat");
vbat = logGetFloat(vbatid);
for (i = 0; i < NBR_LEDS; i++) {
buffer[i][0] = LIMIT(LINSCALE(emptyCharge, fullCharge, 255, 0, vbat)); // Red (emtpy)
buffer[i][1] = 0; // Green
buffer[i][2] = LIMIT(LINSCALE(emptyCharge, fullCharge, 0, 255, vbat)); // Blue (charged)
}
}
/**
* An effect mimicking a blue light siren
*/
static void siren(uint8_t buffer[][3], bool reset)
{
int i;
static int tic = 0;
if (reset)
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], part_black);
}
}
if ((tic < 10) && (tic & 1))
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], blue);
}
}
else
{
for (i=0; i<NBR_LEDS; i++) {
COPY_COLOR(buffer[i], part_black);
}
}
if (++tic >= 20) tic = 0;
}
/**************** Effect list ***************/
Ledring12Effect effectsFct[] =
{
blackEffect,
whiteSpinEffect,
colorSpinEffect,
tiltEffect,
brightnessEffect,
spinEffect2,
doubleSpinEffect,
solidColorEffect,
ledTestEffect,
batteryChargeEffect,
boatEffect,
siren,
gravityLight,
virtualMemEffect,
}; //TODO Add more
/********** Ring init and switching **********/
static xTimerHandle timer;
void ledring12Worker(void * data)
{
static int current_effect = 0;
static uint8_t buffer[NBR_LEDS][3];
bool reset = true;
if (/*!pmIsDischarging() ||*/ (effect > neffect)) {
ws2812Send(black, NBR_LEDS);
return;
}
if (current_effect != effect) {
reset = true;
} else {
reset = false;
}
current_effect = effect;
effectsFct[current_effect](buffer, reset);
ws2812Send(buffer, NBR_LEDS);
}
static void ledring12Timer(xTimerHandle timer)
{
workerSchedule(ledring12Worker, NULL);
setHeadlightsOn(headlightEnable);
}
static void ledring12Init(DeckInfo *info)
{
GPIO_InitTypeDef GPIO_InitStructure;
ws2812Init();
neffect = sizeof(effectsFct)/sizeof(effectsFct[0])-1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_Init(GPIOB, &GPIO_InitStructure);
timer = xTimerCreate( "ringTimer", M2T(50),
pdTRUE, NULL, ledring12Timer );
xTimerStart(timer, 100);
}
PARAM_GROUP_START(ring)
PARAM_ADD(PARAM_UINT8, effect, &effect)
PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect)
PARAM_ADD(PARAM_UINT8, solidRed, &solidRed)
PARAM_ADD(PARAM_UINT8, solidGreen, &solidGreen)
PARAM_ADD(PARAM_UINT8, solidBlue, &solidBlue)
PARAM_ADD(PARAM_UINT8, headlightEnable, &headlightEnable)
PARAM_ADD(PARAM_FLOAT, glowstep, &glowstep)
PARAM_ADD(PARAM_FLOAT, emptyCharge, &emptyCharge)
PARAM_ADD(PARAM_FLOAT, fullCharge, &fullCharge)
PARAM_GROUP_STOP(ring)
static const DeckDriver ledring12_deck = {
.vid = 0xBC,
.pid = 0x01,
.name = "bcLedRing",
.usedPeriph = DECK_USING_TIMER3,
.usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3,
.init = ledring12Init,
};
DECK_DRIVER(ledring12_deck);
crazyflie-firmware-src/src/deck/drivers/src/locodeck.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2016 Bitcraze AB
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* locodeck.c: Dwm1000 deck driver.
*/
#define DEBUG_MODULE "DWM"
#include <stdint.h>
#include <string.h>
#include "stm32fxxx.h"
#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"
#include "deck.h"
#include "system.h"
#include "debug.h"
#include "log.h"
#include "param.h"
#include "nvicconf.h"
#include "locodeck.h"
#if LPS_TDOA_ENABLE
#include "lpsTdoaTag.h"
#else
#include "lpsTwrTag.h"
#endif
#define CS_PIN DECK_GPIO_IO1
#if LPS_TDOA_ENABLE
#define RX_TIMEOUT 10000
#else
#define RX_TIMEOUT 1000
#endif
#define ANTENNA_OFFSET 154.6 // In meter
// The anchor position can be set using parameters
// As an option you can set a static position in this file and set
// anchorPositionOk to enable sending the anchor rangings to the Kalman filter
static lpsAlgoOptions_t algoOptions = {
.tagAddress = 0xbccf000000000008,
.anchorAddress = {
0xbccf000000000001,
0xbccf000000000002,
0xbccf000000000003,
0xbccf000000000004,
0xbccf000000000005,
0xbccf000000000006
},
.antennaDelay = (ANTENNA_OFFSET*499.2e6*128)/299792458.0, // In radio tick
.rangingFailedThreshold = 6,
.anchorPositionOk = false,
// To set a static anchor position from startup, uncomment and modify the
// following code:
/*
.anchorPosition = {
{x: 0.99, y: 1.49, z: 1.80},
{x: 0.99, y: 3.29, z: 1.80},
{x: 4.67, y: 2.54, z: 1.80},
{x: 0.59, y: 2.27, z: 0.20},
{x: 4.70, y: 3.38, z: 0.20},
{x: 4.70, y: 1.14, z: 0.20},
},
.anchorPositionOk = true,
*/
};
#if LPS_TDOA_ENABLE
static uwbAlgorithm_t *algorithm = &uwbTdoaTagAlgorithm;
#else
static uwbAlgorithm_t *algorithm = &uwbTwrTagAlgorithm;
#endif
static bool isInit = false;
static xSemaphoreHandle spiSemaphore;
static SemaphoreHandle_t irqSemaphore;
static dwDevice_t dwm_device;
static dwDevice_t *dwm = &dwm_device;
static uint32_t timeout;
static void txCallback(dwDevice_t *dev)
{
timeout = algorithm->onEvent(dev, eventPacketSent);
}
static void rxCallback(dwDevice_t *dev)
{
timeout = algorithm->onEvent(dev, eventPacketReceived);
}
static void rxTimeoutCallback(dwDevice_t * dev) {
timeout = algorithm->onEvent(dev, eventReceiveTimeout);
}
// static void rxfailedcallback(dwDevice_t *dev) {
// timeout = algorithm->onEvent(dev, eventReceiveFailed);
// }
static void uwbTask(void* parameters)
{
systemWaitStart();
algorithm->init(dwm, &algoOptions);
while(1) {
if (xSemaphoreTake(irqSemaphore, timeout/portTICK_PERIOD_MS)) {
do{
dwHandleInterrupt(dwm);
} while(digitalRead(DECK_GPIO_RX1) != 0);
} else {
timeout = algorithm->onEvent(dwm, eventTimeout);
}
}
}
static uint8_t spiTxBuffer[196];
static uint8_t spiRxBuffer[196];
/************ Low level ops for libdw **********/
static void spiWrite(dwDevice_t* dev, const void *header, size_t headerLength,
const void* data, size_t dataLength)
{
xSemaphoreTake(spiSemaphore, portMAX_DELAY);
digitalWrite(CS_PIN, LOW);
memcpy(spiTxBuffer, header, headerLength);
memcpy(spiTxBuffer+headerLength, data, dataLength);
spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
digitalWrite(CS_PIN, HIGH);
xSemaphoreGive(spiSemaphore);
}
static void spiRead(dwDevice_t* dev, const void *header, size_t headerLength,
void* data, size_t dataLength)
{
xSemaphoreTake(spiSemaphore, portMAX_DELAY);
digitalWrite(CS_PIN, LOW);
memcpy(spiTxBuffer, header, headerLength);
memset(spiTxBuffer+headerLength, 0, dataLength);
spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
memcpy(data, spiRxBuffer+headerLength, dataLength);
digitalWrite(CS_PIN, HIGH);
xSemaphoreGive(spiSemaphore);
}
void __attribute__((used)) EXTI11_Callback(void)
{
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
EXTI_ClearITPendingBit(EXTI_Line11);
//To unlock RadioTask
xSemaphoreGiveFromISR(irqSemaphore, &xHigherPriorityTaskWoken);
if(xHigherPriorityTaskWoken)
portYIELD();
}
static void spiSetSpeed(dwDevice_t* dev, dwSpiSpeed_t speed)
{
if (speed == dwSpiSpeedLow)
{
spiConfigureSlow();
}
else if (speed == dwSpiSpeedHigh)
{
spiConfigureFast();
}
}
static void delayms(dwDevice_t* dev, unsigned int delay)
{
vTaskDelay(M2T(delay));
}
static dwOps_t dwOps = {
.spiRead = spiRead,
.spiWrite = spiWrite,
.spiSetSpeed = spiSetSpeed,
.delayms = delayms,
};
/*********** Deck driver initialization ***************/
static void dwm1000Init(DeckInfo *info)
{
EXTI_InitTypeDef EXTI_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
spiBegin();
// Init IRQ input
bzero(&GPIO_InitStructure, sizeof(GPIO_InitStructure));
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
GPIO_Init(GPIOC, &GPIO_InitStructure);
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource11);
EXTI_InitStructure.EXTI_Line = EXTI_Line11;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
// Init reset output
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Init CS pin
pinMode(CS_PIN, OUTPUT);
// Reset the DW1000 chip
GPIO_WriteBit(GPIOC, GPIO_Pin_10, 0);
vTaskDelay(M2T(10));
GPIO_WriteBit(GPIOC, GPIO_Pin_10, 1);
vTaskDelay(M2T(10));
// Semaphore that protect the SPI communication
spiSemaphore = xSemaphoreCreateMutex();
// Initialize the driver
dwInit(dwm, &dwOps); // Init libdw
int result = dwConfigure(dwm);
if (result != 0) {
isInit = false;
DEBUG_PRINT("Failed to configure DW1000!\r\n");
return;
}
dwEnableAllLeds(dwm);
dwTime_t delay = {.full = 0};
dwSetAntenaDelay(dwm, delay);
dwAttachSentHandler(dwm, txCallback);
dwAttachReceivedHandler(dwm, rxCallback);
dwAttachReceiveTimeoutHandler(dwm, rxTimeoutCallback);
dwNewConfiguration(dwm);
dwSetDefaults(dwm);
dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
dwSetChannel(dwm, CHANNEL_2);
dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);
dwSetReceiveWaitTimeout(dwm, RX_TIMEOUT);
dwCommitConfiguration(dwm);
// Enable interrupt
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
vSemaphoreCreateBinary(irqSemaphore);
xTaskCreate(uwbTask, "lps", 3*configMINIMAL_STACK_SIZE, NULL,
5/*priority*/, NULL);
isInit = true;
}
static bool dwm1000Test()
{
if (!isInit) {
DEBUG_PRINT("Error while initializing DWM1000\n");
}
return isInit;
}
static const DeckDriver dwm1000_deck = {
.vid = 0xBC,
.pid = 0x06,
.name = "bcDWM1000",
.usedGpio = 0, // FIXME: set the used pins
.init = dwm1000Init,
.test = dwm1000Test,
};
DECK_DRIVER(dwm1000_deck);
LOG_GROUP_START(ranging)
LOG_ADD(LOG_FLOAT, distance1, &algoOptions.distance[0])
LOG_ADD(LOG_FLOAT, distance2, &algoOptions.distance[1])
LOG_ADD(LOG_FLOAT, distance3, &algoOptions.distance[2])
LOG_ADD(LOG_FLOAT, distance4, &algoOptions.distance[3])
LOG_ADD(LOG_FLOAT, distance5, &algoOptions.distance[4])
LOG_ADD(LOG_FLOAT, distance6, &algoOptions.distance[5])
LOG_ADD(LOG_FLOAT, distance7, &algoOptions.distance[6])
LOG_ADD(LOG_FLOAT, distance8, &algoOptions.distance[7])
LOG_ADD(LOG_FLOAT, pressure1, &algoOptions.pressures[0])
LOG_ADD(LOG_FLOAT, pressure2, &algoOptions.pressures[1])
LOG_ADD(LOG_FLOAT, pressure3, &algoOptions.pressures[2])
LOG_ADD(LOG_FLOAT, pressure4, &algoOptions.pressures[3])
LOG_ADD(LOG_FLOAT, pressure5, &algoOptions.pressures[4])
LOG_ADD(LOG_FLOAT, pressure6, &algoOptions.pressures[5])
LOG_ADD(LOG_FLOAT, pressure7, &algoOptions.pressures[6])
LOG_ADD(LOG_FLOAT, pressure8, &algoOptions.pressures[7])
LOG_ADD(LOG_UINT16, state, &algoOptions.rangingState)
LOG_GROUP_STOP(ranging)
PARAM_GROUP_START(anchorpos)
PARAM_ADD(PARAM_FLOAT, anchor0x, &algoOptions.anchorPosition[0].x)
PARAM_ADD(PARAM_FLOAT, anchor0y, &algoOptions.anchorPosition[0].y)
PARAM_ADD(PARAM_FLOAT, anchor0z, &algoOptions.anchorPosition[0].z)
PARAM_ADD(PARAM_FLOAT, anchor1x, &algoOptions.anchorPosition[1].x)
PARAM_ADD(PARAM_FLOAT, anchor1y, &algoOptions.anchorPosition[1].y)
PARAM_ADD(PARAM_FLOAT, anchor1z, &algoOptions.anchorPosition[1].z)
PARAM_ADD(PARAM_FLOAT, anchor2x, &algoOptions.anchorPosition[2].x)
PARAM_ADD(PARAM_FLOAT, anchor2y, &algoOptions.anchorPosition[2].y)
PARAM_ADD(PARAM_FLOAT, anchor2z, &algoOptions.anchorPosition[2].z)
PARAM_ADD(PARAM_FLOAT, anchor3x, &algoOptions.anchorPosition[3].x)
PARAM_ADD(PARAM_FLOAT, anchor3y, &algoOptions.anchorPosition[3].y)
PARAM_ADD(PARAM_FLOAT, anchor3z, &algoOptions.anchorPosition[3].z)
PARAM_ADD(PARAM_FLOAT, anchor4x, &algoOptions.anchorPosition[4].x)
PARAM_ADD(PARAM_FLOAT, anchor4y, &algoOptions.anchorPosition[4].y)
PARAM_ADD(PARAM_FLOAT, anchor4z, &algoOptions.anchorPosition[4].z)
PARAM_ADD(PARAM_FLOAT, anchor5x, &algoOptions.anchorPosition[5].x)
PARAM_ADD(PARAM_FLOAT, anchor5y, &algoOptions.anchorPosition[5].y)
PARAM_ADD(PARAM_FLOAT, anchor5z, &algoOptions.anchorPosition[5].z)
PARAM_ADD(PARAM_UINT8, enable, &algoOptions.anchorPositionOk)
PARAM_GROUP_STOP(anchorpos)
crazyflie-firmware-src/src/deck/drivers/src/lpsTdoaTag.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* LPS node firmware.
*
* Copyright 2016, Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* lpsTdoaTag.c 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 lpsTdoaTag.c. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include "log.h"
#include "lpsTdoaTag.h"
#include "stabilizer_types.h"
#include "cfassert.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
#endif // ESTIMATOR_TYPE_kalman
static lpsAlgoOptions_t* options;
static float uwbTdoaDistDiff[LOCODECK_NR_OF_ANCHORS];
static uint8_t previousAnchor;
static rangePacket_t rxPacketBuffer[LOCODECK_NR_OF_ANCHORS];
static dwTime_t arrivals[LOCODECK_NR_OF_ANCHORS];
static double frameTime_in_cl_A[LOCODECK_NR_OF_ANCHORS];
static double clockCorrection_T_To_A[LOCODECK_NR_OF_ANCHORS];
#define MEASUREMENT_NOISE_STD 0.15f
// The maximum diff in distances that we consider to be valid
// Used to sanity check results and remove results that are wrong due to packet loss
#define MAX_DISTANCE_DIFF (5.0f)
static uint32_t statsReceivedPackets = 0;
static uint32_t statsAcceptedAnchorDataPackets = 0;
static uint32_t statsAcceptedPackets = 0;
static uint64_t timestampToUint64(const uint8_t *ts) {
dwTime_t timestamp = {.full = 0};
memcpy(timestamp.raw, ts, sizeof(timestamp.raw));
return timestamp.full;
}
static uint64_t truncateToTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFFFul;
}
#ifdef ESTIMATOR_TYPE_kalman
static void enqueueTDOA(uint8_t anchor1, uint8_t anchor2, double distanceDiff, double timeBetweenMeasurements) {
tdoaMeasurement_t tdoa = {
.stdDev = MEASUREMENT_NOISE_STD,
.distanceDiff = distanceDiff,
.timeBetweenMeasurements = timeBetweenMeasurements,
.anchorPosition[0] = options->anchorPosition[anchor1],
.anchorPosition[1] = options->anchorPosition[anchor2]
};
stateEstimatorEnqueueTDOA(&tdoa);
}
#endif
static double calcClockCorrection(const double frameTime, const double previuosFrameTime) {
double clockCorrection = 1.0;
if (frameTime != 0.0) {
clockCorrection = previuosFrameTime / frameTime;
}
return clockCorrection;
}
// The default receive time in the anchors for messages from other anchors is 0
// and is overwritten with the actual receive time when a packet arrives.
// That is, if no message was received the rx time will be 0.
static bool isValidRxTime(const int64_t anchorRxTime) {
return anchorRxTime != 0;
}
// A note on variable names. They might seem a bit verbose but express quite a lot of information
// We have three actors: Reference anchor (Ar), Anchor n (An) and the deck on the CF called Tag (T)
// rxAr_by_An_in_cl_An should be interpreted as "The time when packet was received from the Referecne Anchor by Anchor N expressed in the clock of Anchor N"
static void rxcallback(dwDevice_t *dev) {
statsReceivedPackets++;
int dataLength = dwGetDataLength(dev);
packet_t rxPacket;
dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
dwTime_t arrival = {.full = 0};
dwGetReceiveTimestamp(dev, &arrival);
const uint8_t anchor = rxPacket.sourceAddress & 0xff;
if (anchor < LOCODECK_NR_OF_ANCHORS) {
const rangePacket_t* packet = (rangePacket_t*)rxPacket.payload;
const int64_t previous_rxAn_by_T_in_cl_T = arrivals[anchor].full;
const int64_t rxAn_by_T_in_cl_T = arrival.full;
const int64_t previous_txAn_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
const int64_t txAn_in_cl_An = timestampToUint64(packet->timestamps[anchor]);
if (anchor != previousAnchor) {
const int64_t previuos_rxAr_by_An_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[previousAnchor]);
const int64_t rxAr_by_An_in_cl_An = timestampToUint64(packet->timestamps[previousAnchor]);
const int64_t rxAn_by_Ar_in_cl_Ar = timestampToUint64(rxPacketBuffer[previousAnchor].timestamps[anchor]);
if (isValidRxTime(previuos_rxAr_by_An_in_cl_An) && isValidRxTime(rxAr_by_An_in_cl_An) && isValidRxTime(rxAn_by_Ar_in_cl_Ar)) {
statsAcceptedAnchorDataPackets++;
// Caclculate clock correction from anchor to reference anchor
const double frameTime_in_cl_An = truncateToTimeStamp(rxAr_by_An_in_cl_An - previuos_rxAr_by_An_in_cl_An);
const double clockCorrection_An_To_Ar = calcClockCorrection(frameTime_in_cl_An, frameTime_in_cl_A[previousAnchor]);
const int64_t rxAr_by_T_in_cl_T = arrivals[previousAnchor].full;
const int64_t txAr_in_cl_Ar = timestampToUint64(rxPacketBuffer[previousAnchor].timestamps[previousAnchor]);
// Calculate distance diff
const int64_t tof_Ar_to_An_in_cl_Ar = (((truncateToTimeStamp(rxAr_by_An_in_cl_An - previous_txAn_in_cl_An) * clockCorrection_An_To_Ar) - truncateToTimeStamp(txAr_in_cl_Ar - rxAn_by_Ar_in_cl_Ar))) / 2.0;
const int64_t delta_txAr_to_txAn_in_cl_Ar = (tof_Ar_to_An_in_cl_Ar + truncateToTimeStamp(txAn_in_cl_An - rxAr_by_An_in_cl_An) * clockCorrection_An_To_Ar);
const int64_t timeDiffOfArrival_in_cl_Ar = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxAr_by_T_in_cl_T) * clockCorrection_T_To_A[previousAnchor] - delta_txAr_to_txAn_in_cl_Ar;
const float tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_Ar / LOCODECK_TS_FREQ;
// Sanity check distances in case of missed packages
if (tdoaDistDiff > -MAX_DISTANCE_DIFF && tdoaDistDiff < MAX_DISTANCE_DIFF) {
uwbTdoaDistDiff[anchor] = tdoaDistDiff;
#ifdef ESTIMATOR_TYPE_kalman
const float timeBetweenMeasurements = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxAr_by_T_in_cl_T) / LOCODECK_TS_FREQ;
enqueueTDOA(previousAnchor, anchor, tdoaDistDiff, timeBetweenMeasurements);
#endif
statsAcceptedPackets++;
}
}
}
// Calculate clock correction for tag to anchor
const double frameTime_in_T = truncateToTimeStamp(rxAn_by_T_in_cl_T - previous_rxAn_by_T_in_cl_T);
frameTime_in_cl_A[anchor] = truncateToTimeStamp(txAn_in_cl_An - previous_txAn_in_cl_An);
clockCorrection_T_To_A[anchor] = calcClockCorrection(frameTime_in_T, frameTime_in_cl_A[anchor]);
arrivals[anchor].full = arrival.full;
memcpy(&rxPacketBuffer[anchor], rxPacket.payload, sizeof(rangePacket_t));
previousAnchor = anchor;
}
}
static void setRadioInReceiveMode(dwDevice_t *dev) {
dwNewReceive(dev);
dwSetDefaults(dev);
dwStartReceive(dev);
}
static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
switch(event) {
case eventPacketReceived:
rxcallback(dev);
setRadioInReceiveMode(dev);
break;
case eventTimeout:
setRadioInReceiveMode(dev);
break;
case eventReceiveTimeout:
setRadioInReceiveMode(dev);
break;
default:
ASSERT_FAILED();
}
return MAX_TIMEOUT;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) {
options = algoOptions;
// Reset module state. Needed by unit tests
memset(rxPacketBuffer, 0, sizeof(rxPacketBuffer));
memset(arrivals, 0, sizeof(arrivals));
memset(frameTime_in_cl_A, 0, sizeof(frameTime_in_cl_A));
memset(clockCorrection_T_To_A, 0, sizeof(clockCorrection_T_To_A));
previousAnchor = 0;
memset(uwbTdoaDistDiff, 0, sizeof(uwbTdoaDistDiff));
statsReceivedPackets = 0;
statsAcceptedAnchorDataPackets = 0;
statsAcceptedPackets = 0;
}
#pragma GCC diagnostic pop
uwbAlgorithm_t uwbTdoaTagAlgorithm = {
.init = Initialize,
.onEvent = onEvent,
};
LOG_GROUP_START(tdoa)
LOG_ADD(LOG_FLOAT, d0, &uwbTdoaDistDiff[0])
LOG_ADD(LOG_FLOAT, d1, &uwbTdoaDistDiff[1])
LOG_ADD(LOG_FLOAT, d2, &uwbTdoaDistDiff[2])
LOG_ADD(LOG_FLOAT, d3, &uwbTdoaDistDiff[3])
LOG_ADD(LOG_FLOAT, d4, &uwbTdoaDistDiff[4])
LOG_ADD(LOG_FLOAT, d5, &uwbTdoaDistDiff[5])
LOG_ADD(LOG_FLOAT, d6, &uwbTdoaDistDiff[6])
LOG_ADD(LOG_FLOAT, d7, &uwbTdoaDistDiff[7])
LOG_ADD(LOG_UINT32, rxCnt, &statsReceivedPackets)
LOG_ADD(LOG_UINT32, anCnt, &statsAcceptedAnchorDataPackets)
LOG_ADD(LOG_UINT32, okCnt, &statsAcceptedPackets)
LOG_GROUP_STOP(tdoa)
crazyflie-firmware-src/src/deck/drivers/src/lpsTwrTag.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* LPS node firmware.
*
* Copyright 2016, Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Foobar 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 Foobar. If not, see <http://www.gnu.org/licenses/>.
*/
/* uwb_twr_anchor.c: Uwb two way ranging anchor implementation */
#include <string.h>
#include "lpsTwrTag.h"
#include "FreeRTOS.h"
#include "task.h"
#include "log.h"
#include "stabilizer_types.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
#include "arm_math.h"
#endif
// Outlier rejection
#ifdef ESTIMATOR_TYPE_kalman
#define RANGING_HISTORY_LENGTH 32
#define OUTLIER_TH 4
static struct {
float32_t history[RANGING_HISTORY_LENGTH];
size_t ptr;
} rangingStats[LOCODECK_NR_OF_ANCHORS];
#endif
// Rangin statistics
static uint32_t rangingPerSec[LOCODECK_NR_OF_ANCHORS];
static float rangingSuccessRate[LOCODECK_NR_OF_ANCHORS];
// Used to calculate above values
static uint32_t succededRanging[LOCODECK_NR_OF_ANCHORS];
static uint32_t failedRanging[LOCODECK_NR_OF_ANCHORS];
// Timestamps for ranging
static dwTime_t poll_tx;
static dwTime_t poll_rx;
static dwTime_t answer_tx;
static dwTime_t answer_rx;
static dwTime_t final_tx;
static dwTime_t final_rx;
static packet_t txPacket;
static volatile uint8_t curr_seq = 0;
static int current_anchor = 0;
static bool ranging_complete = false;
static lpsAlgoOptions_t* options;
static void txcallback(dwDevice_t *dev)
{
dwTime_t departure;
dwGetTransmitTimestamp(dev, &departure);
departure.full += (options->antennaDelay / 2);
switch (txPacket.payload[0]) {
case LPS_TWR_POLL:
poll_tx = departure;
break;
case LPS_TWR_FINAL:
final_tx = departure;
break;
}
}
static uint32_t rxcallback(dwDevice_t *dev) {
dwTime_t arival = { .full=0 };
int dataLength = dwGetDataLength(dev);
if (dataLength == 0) return 0;
packet_t rxPacket;
memset(&rxPacket, 0, MAC802154_HEADER_LENGTH);
dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
if (rxPacket.destAddress != options->tagAddress) {
dwNewReceive(dev);
dwSetDefaults(dev);
dwStartReceive(dev);
return MAX_TIMEOUT;
}
txPacket.destAddress = rxPacket.sourceAddress;
txPacket.sourceAddress = rxPacket.destAddress;
switch(rxPacket.payload[LPS_TWR_TYPE]) {
// Tag received messages
case LPS_TWR_ANSWER:
if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
return 0;
}
txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_FINAL;
txPacket.payload[LPS_TWR_SEQ] = rxPacket.payload[LPS_TWR_SEQ];
dwGetReceiveTimestamp(dev, &arival);
arival.full -= (options->antennaDelay / 2);
answer_rx = arival;
dwNewTransmit(dev);
dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
dwWaitForResponse(dev, true);
dwStartTransmit(dev);
break;
case LPS_TWR_REPORT:
{
lpsTwrTagReportPayload_t *report = (lpsTwrTagReportPayload_t *)(rxPacket.payload+2);
double tround1, treply1, treply2, tround2, tprop_ctn, tprop;
if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
return 0;
}
memcpy(&poll_rx, &report->pollRx, 5);
memcpy(&answer_tx, &report->answerTx, 5);
memcpy(&final_rx, &report->finalRx, 5);
tround1 = answer_rx.low32 - poll_tx.low32;
treply1 = answer_tx.low32 - poll_rx.low32;
tround2 = final_rx.low32 - answer_tx.low32;
treply2 = final_tx.low32 - answer_rx.low32;
tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);
tprop = tprop_ctn / LOCODECK_TS_FREQ;
options->distance[current_anchor] = SPEED_OF_LIGHT * tprop;
options->pressures[current_anchor] = report->asl;
#ifdef ESTIMATOR_TYPE_kalman
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
float32_t mean;
float32_t stddev;
arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
float32_t diff = fabsf(mean - options->distance[current_anchor]);
rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];
if (options->anchorPositionOk && (diff < (OUTLIER_TH*stddev))) {
distanceMeasurement_t dist;
dist.distance = options->distance[current_anchor];
dist.x = options->anchorPosition[current_anchor].x;
dist.y = options->anchorPosition[current_anchor].y;
dist.z = options->anchorPosition[current_anchor].z;
dist.stdDev = 0.25;
stateEstimatorEnqueueDistance(&dist);
}
#endif
ranging_complete = true;
return 0;
break;
}
}
return MAX_TIMEOUT;
}
void initiateRanging(dwDevice_t *dev)
{
current_anchor ++;
if (current_anchor >= LOCODECK_NR_OF_ANCHORS) {
current_anchor = 0;
}
dwIdle(dev);
txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_POLL;
txPacket.payload[LPS_TWR_SEQ] = ++curr_seq;
txPacket.sourceAddress = options->tagAddress;
txPacket.destAddress = options->anchorAddress[current_anchor];
dwNewTransmit(dev);
dwSetDefaults(dev);
dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
dwWaitForResponse(dev, true);
dwStartTransmit(dev);
}
static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
{
static uint32_t statisticStartTick = 0;
if (statisticStartTick == 0) {
statisticStartTick = xTaskGetTickCount();
}
switch(event) {
case eventPacketReceived:
return rxcallback(dev);
break;
case eventPacketSent:
txcallback(dev);
return MAX_TIMEOUT;
break;
case eventTimeout: // Comes back to timeout after each ranging attempt
if (!ranging_complete) {
options->rangingState &= ~(1<<current_anchor);
if (options->failedRanging[current_anchor] < options->rangingFailedThreshold) {
options->failedRanging[current_anchor] ++;
options->rangingState |= (1<<current_anchor);
}
failedRanging[current_anchor]++;
} else {
options->rangingState |= (1<<current_anchor);
options->failedRanging[current_anchor] = 0;
succededRanging[current_anchor]++;
}
// Handle ranging statistic
if (xTaskGetTickCount() > (statisticStartTick+1000)) {
statisticStartTick = xTaskGetTickCount();
for (int i=0; i<LOCODECK_NR_OF_ANCHORS; i++) {
rangingPerSec[i] = failedRanging[i] + succededRanging[i];
if (rangingPerSec[i] > 0) {
rangingSuccessRate[i] = 100.0f*(float)succededRanging[i] / (float)rangingPerSec[i];
} else {
rangingSuccessRate[i] = 0.0f;
}
failedRanging[i] = 0;
succededRanging[i] = 0;
}
}
ranging_complete = false;
initiateRanging(dev);
return MAX_TIMEOUT;
break;
case eventReceiveTimeout:
case eventReceiveFailed:
return 0;
break;
default:
configASSERT(false);
}
return MAX_TIMEOUT;
}
static void twrTagInit(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions)
{
options = algoOptions;
// Initialize the packet in the TX buffer
memset(&txPacket, 0, sizeof(txPacket));
MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
txPacket.pan = 0xbccf;
memset(&poll_tx, 0, sizeof(poll_tx));
memset(&poll_rx, 0, sizeof(poll_rx));
memset(&answer_tx, 0, sizeof(answer_tx));
memset(&answer_rx, 0, sizeof(answer_rx));
memset(&final_tx, 0, sizeof(final_tx));
memset(&final_rx, 0, sizeof(final_rx));
curr_seq = 0;
current_anchor = 0;
options->rangingState = 0;
ranging_complete = false;
memset(options->distance, 0, sizeof(options->distance));
memset(options->pressures, 0, sizeof(options->pressures));
memset(options->failedRanging, 0, sizeof(options->failedRanging));
}
uwbAlgorithm_t uwbTwrTagAlgorithm = {
.init = twrTagInit,
.onEvent = twrTagOnEvent,
};
LOG_GROUP_START(twr)
LOG_ADD(LOG_FLOAT, rangingSuccessRate0, &rangingSuccessRate[0])
LOG_ADD(LOG_UINT32, rangingPerSec0, &rangingPerSec[0])
LOG_GROUP_STOP(twr)
crazyflie-firmware-src/src/deck/drivers/src/lpsTwrTdmaTag.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* LPS node firmware.
*
* Copyright 2016, Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Foobar 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 Foobar. If not, see <http://www.gnu.org/licenses/>.
*/
/* uwb_twr_anchor.c: Uwb two way ranging tag with TDMA implementation */
#include "locodeck.h"
#include <string.h>
#include "log.h"
#include "param.h"
#include "libdw1000.h"
#include "mac.h"
#include "stabilizer_types.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
#include "arm_math.h"
#endif
#define RX_TIMEOUT 1000
// TDMA handling
bool tdmaSynchronized;
dwTime_t frameStart;
#ifndef TDMA_NSLOTS_BITS
#define TDMA_NSLOTS_BITS 1
#endif
#ifndef TDMA_SLOT
#define TDMA_SLOT 0
#endif
#define TDMA_SLOT_BITS 27
#define TDMA_FRAME_BITS (TDMA_SLOT_BITS + TDMA_NSLOTS_BITS)
#define TDMA_SLOT_LEN (1ull<<(TDMA_SLOT_BITS+1))
#define TDMA_FRAME_LEN (1ull<<(TDMA_FRAME_BITS+1))
#define TDMA_LAST_FRAME(NOW) ( NOW & ~(TDMA_FRAME_LEN-1) )
// Amount of failed ranging before the ranging value is set as wrong
#define RANGING_FAILED_TH 6
#define N_ANCHORS 6
int anchors[N_ANCHORS] = {1,2,3,4,5,6};
#define TAG_ADDRESS 8+TDMA_SLOT
// Outlier rejection
#ifdef ESTIMATOR_TYPE_kalman
#define RANGING_HISTORY_LENGTH 32
#define OUTLIER_TH 4
static struct {
float32_t history[RANGING_HISTORY_LENGTH];
size_t ptr;
} rangingStats[N_ANCHORS];
#endif
static uint8_t tag_address[] = {TAG_ADDRESS,0,0,0,0,0,0xcf,0xbc};
static uint8_t base_address[] = {0,0,0,0,0,0,0xcf,0xbc};
// The four packets for ranging
#define POLL 0x01 // Poll is initiated by the tag
#define ANSWER 0x02
#define FINAL 0x03
#define REPORT 0x04 // Report contains all measurement from the anchor
typedef struct {
uint8_t pollRx[5];
uint8_t answerTx[5];
uint8_t finalRx[5];
float pressure;
float temperature;
float asl;
uint8_t pressure_ok;
} __attribute__((packed)) reportPayload_t;
// Timestamps for ranging
static dwTime_t poll_tx;
static dwTime_t poll_rx;
static dwTime_t answer_tx;
static dwTime_t answer_rx;
static dwTime_t final_tx;
static dwTime_t final_rx;
#define ANTENNA_OFFSET 154.6 // In meter
static const uint64_t ANTENNA_DELAY = (ANTENNA_OFFSET*499.2e6*128)/299792458.0; // In radio tick
static packet_t rxPacket;
static packet_t txPacket;
static volatile uint8_t curr_seq = 0;
static int current_anchor = 0;
static bool ranging_complete = false;
static lpsAlgoOptions_t* options;
static void txcallback(dwDevice_t *dev)
{
dwTime_t departure;
dwGetTransmitTimestamp(dev, &departure);
departure.full += (options->antennaDelay / 2);
switch (txPacket.payload[0]) {
case POLL:
poll_tx = departure;
break;
case FINAL:
final_tx = departure;
break;
}
}
#define TYPE 0
#define SEQ 1
static uint32_t rxcallback(dwDevice_t *dev) {
dwTime_t arival = { .full=0 };
int dataLength = dwGetDataLength(dev);
if (dataLength == 0) return 0;
bzero(&rxPacket, MAC802154_HEADER_LENGTH);
dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
if (memcmp(&rxPacket.destAddress, tag_address, 8)) {
dwNewReceive(dev);
dwSetDefaults(dev);
dwStartReceive(dev);
return MAX_TIMEOUT;
}
txPacket.destAddress = rxPacket.sourceAddress;
txPacket.sourceAddress = rxPacket.destAddress;
switch(rxPacket.payload[TYPE]) {
// Tag received messages
case ANSWER:
if (rxPacket.payload[SEQ] != curr_seq) {
return 0;
}
txPacket.payload[0] = FINAL;
txPacket.payload[SEQ] = rxPacket.payload[SEQ];
dwGetReceiveTimestamp(dev, &arival);
arival.full -= (ANTENNA_DELAY/2);
answer_rx = arival;
dwNewTransmit(dev);
dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
dwWaitForResponse(dev, true);
dwStartTransmit(dev);
break;
case REPORT:
{
reportPayload_t *report = (reportPayload_t *)(rxPacket.payload+2);
double tround1, treply1, treply2, tround2, tprop_ctn, tprop;
if (rxPacket.payload[SEQ] != curr_seq) {
return 0;
}
memcpy(&poll_rx, &report->pollRx, 5);
memcpy(&answer_tx, &report->answerTx, 5);
memcpy(&final_rx, &report->finalRx, 5);
tround1 = answer_rx.low32 - poll_tx.low32;
treply1 = answer_tx.low32 - poll_rx.low32;
tround2 = final_rx.low32 - answer_tx.low32;
treply2 = final_tx.low32 - answer_rx.low32;
tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);
tprop = tprop_ctn/LOCODECK_TS_FREQ;
options->distance[current_anchor] = SPEED_OF_LIGHT * tprop;
options->pressures[current_anchor] = report->asl;
#ifdef ESTIMATOR_TYPE_kalman
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
float32_t mean;
float32_t stddev;
arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
float32_t diff = fabsf(mean - options->distance[current_anchor]);
rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];
if (options->anchorPositionOk && (diff < (OUTLIER_TH*stddev))) {
distanceMeasurement_t dist;
dist.distance = options->distance[current_anchor];
dist.x = options->anchorPosition[current_anchor].x;
dist.y = options->anchorPosition[current_anchor].y;
dist.z = options->anchorPosition[current_anchor].z;
dist.stdDev = 0.25;
stateEstimatorEnqueueDistance(&dist);
}
#endif
if (current_anchor == 0) {
// Final packet is sent by us and received by the anchor
// We use it as synchonisation time for TDMA
dwTime_t offset = { .full =final_tx.full - final_rx.full };
frameStart.full = TDMA_LAST_FRAME(final_rx.full) + offset.full;
tdmaSynchronized = true;
}
ranging_complete = true;
return 0;
break;
}
}
return MAX_TIMEOUT;
}
/* Adjust time for schedule transfer by DW1000 radio. Set 9 LSB to 0 */
uint32_t adjustTxRxTime(dwTime_t *time)
{
uint32_t added = (1<<9) - (time->low32 & ((1<<9)-1));
time->low32 = (time->low32 & ~((1<<9)-1)) + (1<<9);
return added;
}
/* Calculate the transmit time for a given timeslot in the current frame */
static dwTime_t transmitTimeForSlot(int slot)
{
dwTime_t transmitTime = { .full = 0 };
// Calculate start of the slot
transmitTime.full = frameStart.full + slot*TDMA_SLOT_LEN;
// DW1000 can only schedule time with 9 LSB at 0, adjust for it
adjustTxRxTime(&transmitTime);
return transmitTime;
}
static void initiateRanging(dwDevice_t *dev)
{
if (tdmaSynchronized) {
// go to next frame
frameStart.full += TDMA_FRAME_LEN;
current_anchor ++;
if (current_anchor >= N_ANCHORS) {
current_anchor = 0;
}
} else {
current_anchor = 0;
}
base_address[0] = anchors[current_anchor];
dwIdle(dev);
txPacket.payload[TYPE] = POLL;
txPacket.payload[SEQ] = ++curr_seq;
memcpy(&txPacket.sourceAddress, &tag_address, 8);
memcpy(&txPacket.destAddress, &base_address, 8);
dwNewTransmit(dev);
dwSetDefaults(dev);
dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
if (tdmaSynchronized) {
dwTime_t txTime = transmitTimeForSlot(TDMA_SLOT);
dwSetTxRxTime(dev, txTime);
}
dwWaitForResponse(dev, true);
dwStartTransmit(dev);
}
static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
{
switch(event) {
case eventPacketReceived:
return rxcallback(dev);
break;
case eventPacketSent:
txcallback(dev);
return MAX_TIMEOUT;
break;
case eventTimeout: // Comes back to timeout after each ranging attempt
if (!ranging_complete) {
options->rangingState &= ~(1<<current_anchor);
if (options->failedRanging[current_anchor] < RANGING_FAILED_TH) {
options->failedRanging[current_anchor] ++;
options->rangingState |= (1<<current_anchor);
}
} else {
options->rangingState |= (1<<current_anchor);
options->failedRanging[current_anchor] = 0;
}
ranging_complete = false;
initiateRanging(dev);
return MAX_TIMEOUT;
break;
case eventReceiveTimeout:
case eventReceiveFailed:
return 0;
break;
default:
configASSERT(false);
}
return MAX_TIMEOUT;
}
static void twrTagInit(dwDevice_t *dev, lpsAlgoOptions_t* newOptions)
{
tdmaSynchronized = false;
options = newOptions;
// Initialize the packet in the TX buffer
MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
txPacket.pan = 0xbccf;
// onEvent is going to be called with eventTimeout which will start ranging
}
uwbAlgorithm_t uwbTwrTagAlgorithm = {
.init = twrTagInit,
.onEvent = twrTagOnEvent,
};
crazyflie-firmware-src/src/deck/drivers/src/rzr.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* rzr.c - Crazyflie RZR board.
*/
#define DEBUG_MODULE "RZR"
#include <stdint.h>
#include <string.h>
#include "stm32fxxx.h"
#include "config.h"
#include "motors.h"
#include "debug.h"
#include "deck.h"
#include "extrx.h"
#include "pm.h"
#define RZR_GPIO_RCC_M1_OVERRIDE RCC_AHB1Periph_GPIOA
#define RZR_GPIO_PORT_M1_OVERRIDE GPIOA
#define RZR_GPIO_PIN_M1_OVERRIDE GPIO_Pin_0
#define RZR_GPIO_RCC_M2_OVERRIDE RCC_AHB1Periph_GPIOB
#define RZR_GPIO_PORT_M2_OVERRIDE GPIOB
#define RZR_GPIO_PIN_M2_OVERRIDE GPIO_Pin_12
#define RZR_GPIO_RCC_M3_OVERRIDE RCC_AHB1Periph_GPIOC
#define RZR_GPIO_PORT_M3_OVERRIDE GPIOC
#define RZR_GPIO_PIN_M3_OVERRIDE GPIO_Pin_8
#define RZR_GPIO_RCC_M4_OVERRIDE RCC_AHB1Periph_GPIOC
#define RZR_GPIO_PORT_M4_OVERRIDE GPIOC
#define RZR_GPIO_PIN_M4_OVERRIDE GPIO_Pin_15
//Hardware configuration
static bool isInit;
static void rzrInit(DeckInfo *info)
{
GPIO_InitTypeDef GPIO_InitStructure = {0};
if(isInit)
return;
DEBUG_PRINT("Switching to brushless.\n");
// Configure GPIO for power to BL motor connectors
RCC_AHB1PeriphClockCmd(RZR_GPIO_RCC_M1_OVERRIDE, ENABLE);
RCC_AHB1PeriphClockCmd(RZR_GPIO_RCC_M2_OVERRIDE, ENABLE);
RCC_AHB1PeriphClockCmd(RZR_GPIO_RCC_M3_OVERRIDE, ENABLE);
RCC_AHB1PeriphClockCmd(RZR_GPIO_RCC_M4_OVERRIDE, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Pin = RZR_GPIO_PIN_M1_OVERRIDE;
GPIO_Init(RZR_GPIO_PORT_M1_OVERRIDE, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = RZR_GPIO_PIN_M2_OVERRIDE;
GPIO_Init(RZR_GPIO_PORT_M2_OVERRIDE, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = RZR_GPIO_PIN_M3_OVERRIDE;
GPIO_Init(RZR_GPIO_PORT_M3_OVERRIDE, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = RZR_GPIO_PIN_M4_OVERRIDE;
GPIO_Init(RZR_GPIO_PORT_M4_OVERRIDE, &GPIO_InitStructure);
// Enable for power to BL motor connectors
GPIO_WriteBit(RZR_GPIO_PORT_M1_OVERRIDE, RZR_GPIO_PIN_M1_OVERRIDE, 1);
GPIO_WriteBit(RZR_GPIO_PORT_M2_OVERRIDE, RZR_GPIO_PIN_M2_OVERRIDE, 1);
GPIO_WriteBit(RZR_GPIO_PORT_M3_OVERRIDE, RZR_GPIO_PIN_M3_OVERRIDE, 1);
GPIO_WriteBit(RZR_GPIO_PORT_M4_OVERRIDE, RZR_GPIO_PIN_M4_OVERRIDE, 1);
// Remap motor PWM output
motorsInit(motorMapRZRBrushless);
isInit = true;
}
static bool rzrTest()
{
bool status = true;
if(!isInit)
return false;
status = motorsTest();
return status;
}
static const DeckDriver rzr_deck = {
// .vid = 0xBC,
// .pid = 0x08,
.name = "bcRZR",
.usedPeriph = 0,
.usedGpio = 0,
.init = rzrInit,
.test = rzrTest,
};
DECK_DRIVER(rzr_deck);
crazyflie-firmware-src/src/deck/drivers/src/test/bigquadtest.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* bigquadtest.c - Testing of the Big Quad deck.
*/
#define DEBUG_MODULE "BQ-TEST"
#include <stdint.h>
#include "FreeRTOS.h"
#include "task.h"
#include "stm32fxxx.h"
#include "config.h"
#include "debug.h"
#include "deck.h"
#include "deck_test.h"
#include "param.h"
#define VBAT_TEST_VOLTAGE_LOW (3.0 / ((1.0 + 69.0 + 10.0) / 10.0) * 0.95) /* 0.35625 */
#define VBAT_TEST_VOLTAGE_HIGH (3.0 / ((1.0 + 69.0 + 10.0) / 10.0) * 1.05) /* 0.39375 */
#define TEST(result, str, status) decktestEval(result, DEBUG_MODULE ": " str, status)
static uint8_t testsPass = 0;
static bool bigquadtestRun()
{
bool status = true;
float readVoltage;
GpioRegBuf gpioSaved;
UBaseType_t prioSaved;
/* Raise priority to prevent task switch during testing */
prioSaved = uxTaskPriorityGet(NULL);
vTaskPrioritySet(NULL, configMAX_PRIORITIES - 1);
/* Save GPIO states */
decktestSaveGPIOStatesABC(&gpioSaved);
/* Test SDA and CPPM(MOSI) which should be bridged */
pinMode(DECK_GPIO_SDA, INPUT);
pinMode(DECK_GPIO_MOSI, INPUT);
TEST(digitalRead(DECK_GPIO_SDA) == HIGH, "SDA == HIGH", &status);
TEST(digitalRead(DECK_GPIO_MOSI) == HIGH, "CPPM == HIGH", &status);
pinMode(DECK_GPIO_MOSI, OUTPUT);
digitalWrite(DECK_GPIO_MOSI, LOW);
vTaskDelay(1);
TEST(digitalRead(DECK_GPIO_SDA) == LOW, "SDA == LOW", &status);
pinMode(DECK_GPIO_MOSI, INPUT);
/* Test SCL and BUZ(IO_4) which should be bridged */
pinMode(DECK_GPIO_SCL, INPUT);
pinMode(DECK_GPIO_IO4, INPUT);
TEST(digitalRead(DECK_GPIO_SCL) == HIGH, "SCL == HIGH", &status);
TEST(digitalRead(DECK_GPIO_IO4) == HIGH, "BUZ == HIGH", &status);
pinMode(DECK_GPIO_IO4, OUTPUT);
digitalWrite(DECK_GPIO_IO4, LOW);
vTaskDelay(1);
TEST(digitalRead(DECK_GPIO_SCL) == LOW, "SCL == LOW", &status);
pinMode(DECK_GPIO_IO4, INPUT);
/* Test M1(TX2), M2(IO_3), M3(IO_2), M4(RX2) */
/* which are pulled high with 1k */
pinMode(DECK_GPIO_TX2, INPUT_PULLDOWN);
pinMode(DECK_GPIO_RX2, INPUT_PULLDOWN);
pinMode(DECK_GPIO_IO2, INPUT_PULLDOWN);
pinMode(DECK_GPIO_IO3, INPUT_PULLDOWN);
TEST(digitalRead(DECK_GPIO_TX2) == HIGH, "M1(TX2) == HIGH", &status);
TEST(digitalRead(DECK_GPIO_IO3) == HIGH, "M2(IO_3) == HIGH", &status);
TEST(digitalRead(DECK_GPIO_IO2) == HIGH, "M3(IO_2) == HIGH", &status);
TEST(digitalRead(DECK_GPIO_RX2) == HIGH, "M4(RX2) == HIGH", &status);
/* Test TX and RX which should be bridged */
pinMode(DECK_GPIO_RX1, INPUT_PULLUP);
pinMode(DECK_GPIO_TX1, OUTPUT);
digitalWrite(DECK_GPIO_TX1, LOW);
vTaskDelay(1);
TEST(digitalRead(DECK_GPIO_RX1) == LOW, "TX, RX", &status);
digitalWrite(DECK_GPIO_TX1, HIGH);
vTaskDelay(1);
TEST(digitalRead(DECK_GPIO_RX1) == HIGH, "TX, RX", &status);
pinMode(DECK_GPIO_TX1, INPUT); // restore
/* Test CUR(SCK) and VBAT(MISO) which should be bridged */
/* CUR set to output 3V and VBAT to measure */
pinMode(DECK_GPIO_SCK, OUTPUT);
digitalWrite(DECK_GPIO_SCK, HIGH);
vTaskDelay(10);
readVoltage = analogReadVoltage(DECK_GPIO_MISO);
TEST((readVoltage > VBAT_TEST_VOLTAGE_LOW &&
readVoltage < VBAT_TEST_VOLTAGE_HIGH),
"VBAT(MISO) voltage", &status);
decktestRestoreGPIOStatesABC(&gpioSaved);
if (status)
{
testsPass = 1;
DEBUG_PRINT("BigQuad deck test [OK]\n");
}
/* Restore priority */
vTaskPrioritySet(NULL, prioSaved);
return status;
}
static const DeckDriver bigquadtest_deck = {
.name = "bcBigQuadTest",
.usedPeriph = 0,
.usedGpio = 0,
.test = bigquadtestRun,
};
DECK_DRIVER(bigquadtest_deck);
PARAM_GROUP_START(BigQuadTest)
PARAM_ADD(PARAM_UINT8, pass, &testsPass)
PARAM_GROUP_STOP(BigQuadTest)
crazyflie-firmware-src/src/deck/drivers/src/test/exptest.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* 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, in version 3.
*
* 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/>.
*
* exptest.c - Testing of expansion port.
*/
#define DEBUG_MODULE "ET"
#include <stdint.h>
#include "stm32fxxx.h"
#include "config.h"
#include "debug.h"
#include "deck.h"
#include "deck_test.h"
#include "sensors.h"
//Hardware configuration
#define ET_GPIO_PERIF (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)
#define ET_GPIO_PORT_TX1 GPIOC
#define ET_GPIO_PIN_TX1 GPIO_Pin_10
#define ET_GPIO_PORT_RX1 GPIOC
#define ET_GPIO_PIN_RX1 GPIO_Pin_11
#define ET_GPIO_PORT_TX2 GPIOA
#define ET_GPIO_PIN_TX2 GPIO_Pin_2
#define ET_GPIO_PORT_RX2 GPIOA
#define ET_GPIO_PIN_RX2 GPIO_Pin_3
#define ET_GPIO_PORT_SCK GPIOA
#define ET_GPIO_PIN_SCK GPIO_Pin_5
#define ET_GPIO_PORT_MOSI GPIOA
#define ET_GPIO_PIN_MOSI GPIO_Pin_6
#define ET_GPIO_PORT_MISO GPIOA
#define ET_GPIO_PIN_MISO GPIO_Pin_7
#define ET_GPIO_PORT_SDA GPIOB
#define ET_GPIO_PIN_SDA GPIO_Pin_7
#define ET_GPIO_PORT_SCL GPIOB
#define ET_GPIO_PIN_SCL GPIO_Pin_6
#define ET_GPIO_PORT_IO1 GPIOB
#define ET_GPIO_PIN_IO1 GPIO_Pin_8
#define ET_GPIO_PORT_IO2 GPIOB
#define ET_GPIO_PIN_IO2 GPIO_Pin_5
#define ET_GPIO_PORT_IO3 GPIOB
#define ET_GPIO_PIN_IO3 GPIO_Pin_4
#define ET_GPIO_PORT_IO4 GPIOC
#define ET_GPIO_PIN_IO4 GPIO_Pin_12
#define ET_NBR_PINS 11
#define ET_IO4_PIN (ET_NBR_PINS - 1)
typedef struct _etGpio
{
GPIO_TypeDef *port;
uint16_t pin;
char name[6];
} EtGpio;
EtGpio etGpioIn[ET_NBR_PINS] =
{
{ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
{ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
{ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
{ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
{ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
{ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
{ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
{ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
{ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
{ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
{ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
};
EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
static bool isInit;
static bool exptestTestAllPins(bool test);
static bool exptestTestPin(EtGpio *etPin, bool test);
static bool exptestRun(void)
{
int i;
volatile int delay;
bool status = true;
GPIO_InitTypeDef GPIO_InitStructure;
GpioRegBuf gpioSaved;
isInit = true;
status &= sensorsManufacturingTest();
// Enable GPIOs
RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
decktestSaveGPIOStatesABC(&gpioSaved);
for (i = 0; i < ET_NBR_PINS; i++)
{
//Initialize the pins as inputs
GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
}
for (i = 0; i < ET_NBR_PINS && status; i++)
{
// Configure pin as output to poke others
GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
// Test high
GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
for (delay = 0; delay < 1000; delay++);
if (!exptestTestAllPins(1))
{
status = false;
}
// Test low
GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
for (delay = 0; delay < 1000; delay++);
if (!exptestTestAllPins(0))
{
status = false;
}
// Restore
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
}
decktestRestoreGPIOStatesABC(&gpioSaved);
if (status)
{
// Configure SDA & SCL to turn on OK leds
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
// Turn on OK LEDs.
GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
}
return status;
}
static bool exptestTestAllPins(bool test)
{
int i;
bool status = true;
for (i = 0; i < ET_NBR_PINS; i++)
{
if (!exptestTestPin(&etGpioIn[i], test))
{
status = false;
}
}
return status;
}
static bool exptestTestPin(EtGpio *etPin, bool test)
{
if (test == GPIO_ReadInputDataBit(etPin->port, etPin->pin))
{
return true;
}
else
{
DEBUG_PRINT("Pin:%s != %d [FAIL]\n", etPin->name, test);
return false;
}
}
static const DeckDriver exptest_deck = {
.vid = 0xBC,
.pid = 0xFF,
.name = "bcExpTest",
.usedGpio = 0, // FIXME: Edit the used GPIOs
.test = exptestRun,
};
DECK_DRIVER(exptest_deck);
crazyflie-firmware-src/src/deck/drivers/src/usddeck.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2016 Bitcraze AB
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* usddeck.c: micro SD deck driver. Implements logging to micro SD card.
*/
#define DEBUG_MODULE "uSD"
#include <stdint.h>
#include <string.h>
#include "stm32fxxx.h"
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "ff.h"
#include "fatfs_sd.h"
#include "deck.h"
#include "usddeck.h"
#include "deck_spi.h"
#include "system.h"
#include "debug.h"
#include "led.h"
// Hardware defines
#define USD_CS_PIN DECK_GPIO_IO4
// FATFS low lever driver functions.
static void initSpi(void);
static void setSlowSpiMode(void);
static void setFastSpiMode(void);
static BYTE xchgSpi(BYTE dat);
static void rcvrSpiMulti(BYTE *buff, UINT btr);
static void xmitSpiMulti(const BYTE *buff, UINT btx);
static void csHigh(void);
static void csLow(void);
static BYTE exchangeBuff[512];
#ifdef USD_RUN_DISKIO_FUNCTION_TESTS
DWORD workBuff[512]; /* 2048 byte working buffer */
#endif
static xTimerHandle timer;
static void usdTimer(xTimerHandle timer);
//Fatfs object
FATFS FatFs;
//File object
FIL logFile;
// Low lever driver functions
static sdSpiContext_t sdSpiContext =
{
.initSpi = initSpi,
.setSlowSpiMode = setSlowSpiMode,
.setFastSpiMode = setFastSpiMode,
.xchgSpi = xchgSpi,
.rcvrSpiMulti = rcvrSpiMulti,
.xmitSpiMulti = xmitSpiMulti,
.csLow = csLow,
.csHigh = csHigh,
.stat = STA_NOINIT,
.timer1 = 0,
.timer2 = 0
};
static DISKIO_LowLevelDriver_t fatDrv =
{
SD_disk_initialize,
SD_disk_status,
SD_disk_ioctl,
SD_disk_write,
SD_disk_read,
&sdSpiContext,
};
/*-----------------------------------------------------------------------*/
/* FATFS SPI controls (Platform dependent) */
/*-----------------------------------------------------------------------*/
/* Initialize MMC interface */
static void initSpi(void)
{
spiBegin(); /* Enable SPI function */
pinMode(USD_CS_PIN, OUTPUT);
csHigh();
// FIXME: DELAY of 10ms?
}
static void setSlowSpiMode(void)
{
spiConfigureSlow();
}
static void setFastSpiMode(void)
{
spiConfigureFast();
}
/* Exchange a byte */
static BYTE xchgSpi(BYTE dat)
{
BYTE receive;
spiExchange(1, &dat, &receive);
return (BYTE)receive;
}
/* Receive multiple byte */
static void rcvrSpiMulti(BYTE *buff, UINT btr)
{
memset(exchangeBuff, 0xFFFFFFFF, btr);
spiExchange(btr, exchangeBuff, buff);
}
/* Send multiple byte */
static void xmitSpiMulti(const BYTE *buff, UINT btx)
{
spiExchange(btx, buff, exchangeBuff);
}
static void csHigh(void)
{
digitalWrite(USD_CS_PIN, 1);
}
static void csLow(void)
{
digitalWrite(USD_CS_PIN, 0);
}
/*********** Deck driver initialization ***************/
static bool isInit = false;
static void usdInit(DeckInfo *info)
{
isInit = true;
FATFS_AddDriver(&fatDrv, 0);
timer = xTimerCreate( "usdTimer", M2T(SD_DISK_TIMER_PERIOD_MS), pdTRUE, NULL, usdTimer);
xTimerStart(timer, 0);
}
static bool usdTest()
{
if (!isInit)
{
DEBUG_PRINT("Error while initializing uSD deck\n");
}
#ifdef USD_RUN_DISKIO_FUNCTION_TESTS
int result;
extern int test_diskio (BYTE pdrv, UINT ncyc, DWORD* buff, UINT sz_buff);
result = test_diskio(0, 1, (DWORD*)&workBuff, sizeof(workBuff));
if (result)
{
DEBUG_PRINT("(result=%d)\nFatFs diskio functions test [FAIL].\n", result);
isInit = false;
}
else
{
DEBUG_PRINT("FatFs diskio functions test [OK].\n");
}
#endif
return isInit;
}
static void usdTimer(xTimerHandle timer)
{
SD_disk_timerproc(&sdSpiContext);
}
static const DeckDriver usd_deck = {
.vid = 0xBC,
.pid = 0x08,
.name = "bcUSD",
.usedGpio = DECK_USING_MISO|DECK_USING_MOSI|DECK_USING_SCK|DECK_USING_IO_4,
.usedPeriph = DECK_USING_SPI,
.init = usdInit,
.test = usdTest,
};
DECK_DRIVER(usd_deck);
crazyflie-firmware-src/src/deck/drivers/src/vl53l0x.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* 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, in version 3.
*
* 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/>.
*
* vl53l0x.c: Time-of-flight distance sensor driver
*/
#define DEBUG_MODULE "VLX"
#include "FreeRTOS.h"
#include "task.h"
#include "deck.h"
#include "system.h"
#include "debug.h"
#include "log.h"
#include "i2cdev.h"
#include "vl53l0x.h"
#include "stabilizer_types.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
#include "arm_math.h"
//#define UPDATE_KALMAN_WITH_RANGING // uncomment to push into the kalman
#ifdef UPDATE_KALMAN_WITH_RANGING
#define RANGE_OUTLIER_LIMIT 1500 // the measured range is in [mm]
// Measurement noise model
static float expPointA = 1.0f;
static float expStdA = 0.0025f; // STD at elevation expPointA [m]
static float expPointB = 1.3f;
static float expStdB = 0.2f; // STD at elevation expPointB [m]
static float expCoeff;
#endif // UPDATE_KALMAN_WITH_RANGING
#endif // ESTIMATOR_TYPE_kalman
static uint8_t devAddr;
static I2C_Dev *I2Cx;
static bool isInit;
static uint16_t io_timeout = 0;
static bool did_timeout;
static uint16_t timeout_start_ms;
static uint16_t range_last = 0;
// Record the current time to check an upcoming timeout against
#define startTimeout() (timeout_start_ms = xTaskGetTickCount())
// Check if timeout is enabled (set to nonzero value) and has expired
#define checkTimeoutExpired() (io_timeout > 0 && ((uint16_t)xTaskGetTickCount() - timeout_start_ms) > io_timeout)
// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs
// from register value
// based on VL53L0X_decode_vcsel_period()
#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1)
// Encode VCSEL pulse period register value from period in PCLKs
// based on VL53L0X_encode_vcsel_period()
#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1)
// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs
// based on VL53L0X_calc_macro_period_ps()
// PLL_period_ps = 1655; macro_period_vclks = 2304
#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
// read by init and used when starting measurement;
// is StopVariable field of VL53L0X_DevData_t structure in API
static uint8_t stop_variable;
static uint32_t measurement_timing_budget_us;
static uint16_t measurement_timing_budget_ms;
// Get reference SPAD (single photon avalanche diode) count and type
// based on VL53L0X_get_info_from_device(),
// but only gets reference SPAD count and type
static bool vl53l0xGetSpadInfo(uint8_t * count, bool * type_is_aperture);
// Get sequence step enables
// based on VL53L0X_GetSequenceStepEnables()
static void vl53l0xGetSequenceStepEnables(SequenceStepEnables * enables);
// Get sequence step timeouts
// based on get_sequence_step_timeout(),
// but gets all timeouts instead of just the requested one, and also stores
// intermediate values
static void vl53l0xGetSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts);
// based on VL53L0X_perform_single_ref_calibration()
static bool vl53l0xPerformSingleRefCalibration(uint8_t vhv_init_byte);
// Decode sequence step timeout in MCLKs from register value
// based on VL53L0X_decode_timeout()
// Note: the original function returned a uint32_t, but the return value is
// always stored in a uint16_t.
static uint16_t vl53l0xDecodeTimeout(uint16_t reg_val);
// Encode sequence step timeout register value from timeout in MCLKs
// based on VL53L0X_encode_timeout()
// Note: the original function took a uint16_t, but the argument passed to it
// is always a uint16_t.
static uint16_t vl53l0xEncodeTimeout(uint16_t timeout_mclks);
// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
// based on VL53L0X_calc_timeout_us()
static uint32_t vl53l0xTimeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks);
// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
// based on VL53L0X_calc_timeout_mclks()
static uint32_t vl53l0xTimeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
static uint16_t vl53l0xReadReg16Bit(uint8_t reg);
static bool vl53l0xWriteReg16Bit(uint8_t reg, uint16_t val);
static bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val);
/** Default constructor, uses default I2C address.
* @see VL53L0X_DEFAULT_ADDRESS
*/
void vl53l0xInit(DeckInfo* info)
{
if (isInit)
return;
i2cdevInit(I2C1_DEV);
I2Cx = I2C1_DEV;
devAddr = VL53L0X_DEFAULT_ADDRESS;
xTaskCreate(vl53l0xTask, "vl53l0x", 2*configMINIMAL_STACK_SIZE, NULL, 3, NULL);
#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING)
// pre-compute constant in the measurement noise mdoel
expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
#endif
isInit = true;
}
bool vl53l0xTest(void)
{
bool testStatus;
if (!isInit)
return false;
// Measurement noise model
testStatus = vl53l0xTestConnection();
testStatus &= vl53l0xInitSensor(true);
return testStatus;
}
void vl53l0xTask(void* arg)
{
systemWaitStart();
TickType_t xLastWakeTime;
vl53l0xSetVcselPulsePeriod(VcselPeriodPreRange, 18);
vl53l0xSetVcselPulsePeriod(VcselPeriodFinalRange, 14);
vl53l0xStartContinuous(100);
while (1) {
xLastWakeTime = xTaskGetTickCount();
range_last = vl53l0xReadRangeContinuousMillimeters();
#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING)
// check if range is feasible and push into the kalman filter
// the sensor should not be able to measure >3 [m], and outliers typically
// occur as >8 [m] measurements
if (range_last < RANGE_OUTLIER_LIMIT){
// Form measurement
tofMeasurement_t tofData;
tofData.timestamp = xTaskGetTickCount();
tofData.distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
tofData.stdDev = expStdA * (1.0f + expf( expCoeff * ( tofData.distance - expPointA)));
stateEstimatorEnqueueTOF(&tofData);
}
#endif
vTaskDelayUntil(&xLastWakeTime, M2T(measurement_timing_budget_ms));
}
}
/** Verify the I2C connection.
* Make sure the device is connected and responds as expected.
* @return True if connection is valid, false otherwise
*/
bool vl53l0xTestConnection()
{
bool ret = true;
ret &= vl53l0xGetModelID() == VL53L0X_IDENTIFICATION_MODEL_ID;
ret &= vl53l0xGetRevisionID() == VL53L0X_IDENTIFICATION_REVISION_ID;
return ret;
}
/** Get Model ID.
* This register is used to verify the model number of the device,
* but only before it has been configured to run
* @return Model ID
* @see VL53L0X_RA_IDENTIFICATION_MODEL_ID
* @see VL53L0X_IDENTIFICATION_MODEL_ID
*/
uint16_t vl53l0xGetModelID()
{
return vl53l0xReadReg16Bit(VL53L0X_RA_IDENTIFICATION_MODEL_ID);
}
/** Get Revision ID.
* This register is used to verify the revision number of the device,
* but only before it has been configured to run
* @return Revision ID
* @see VL53L0X_RA_IDENTIFICATION_REVISION_ID
* @see VL53L0X_IDENTIFICATION_REVISION_ID
*/
uint8_t vl53l0xGetRevisionID()
{
uint8_t output = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_IDENTIFICATION_REVISION_ID, &output);
return output;
}
// Initialize sensor using sequence based on VL53L0X_DataInit(),
// VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration().
// This function does not perform reference SPAD calibration
// (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it
// is performed by ST on the bare modules; it seems like that should work well
// enough unless a cover glass is added.
// If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
// mode.
bool vl53l0xInitSensor(bool io_2v8)
{
uint8_t temp;
// VL53L0X_DataInit() begin
// sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
if (io_2v8)
{
i2cdevWriteBit(I2Cx, devAddr, VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0, 0x01);
}
// "Set I2C standard mode"
i2cdevWriteByte(I2Cx, devAddr, 0x88, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevReadByte(I2Cx, devAddr, 0x91, &stop_variable);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x00);
// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, &temp);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, temp | 0x12);
// set final range signal rate limit to 0.25 MCPS (million counts per second)
vl53l0xSetSignalRateLimit(0.25);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xFF);
// VL53L0X_DataInit() end
// VL53L0X_StaticInit() begin
uint8_t spad_count;
bool spad_type_is_aperture;
if (!vl53l0xGetSpadInfo(&spad_count, &spad_type_is_aperture)) { return false; }
// The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
// the API, but the same data seems to be more easily readable from
// GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
uint8_t ref_spad_map[6];
i2cdevRead(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
// -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
uint8_t spads_enabled = 0;
for (uint8_t i = 0; i < 48; i++)
{
if (i < first_spad_to_enable || spads_enabled == spad_count)
{
// This bit is lower than the first one that should be enabled, or
// (reference_spad_count) bits have already been enabled, so zero this bit
ref_spad_map[i / 8] &= ~(1 << (i % 8));
}
else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
{
spads_enabled++;
}
}
i2cdevWrite(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
// -- VL53L0X_set_reference_spads() end
// -- VL53L0X_load_tuning_settings() begin
// DefaultTuningSettings from vl53l0x_tuning.h
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x09, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x10, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x11, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x24, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x25, 0xFF);
i2cdevWriteByte(I2Cx, devAddr, 0x75, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x4E, 0x2C);
i2cdevWriteByte(I2Cx, devAddr, 0x48, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x30, 0x20);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x30, 0x09);
i2cdevWriteByte(I2Cx, devAddr, 0x54, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x31, 0x04);
i2cdevWriteByte(I2Cx, devAddr, 0x32, 0x03);
i2cdevWriteByte(I2Cx, devAddr, 0x40, 0x83);
i2cdevWriteByte(I2Cx, devAddr, 0x46, 0x25);
i2cdevWriteByte(I2Cx, devAddr, 0x60, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x27, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x50, 0x06);
i2cdevWriteByte(I2Cx, devAddr, 0x51, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x52, 0x96);
i2cdevWriteByte(I2Cx, devAddr, 0x56, 0x08);
i2cdevWriteByte(I2Cx, devAddr, 0x57, 0x30);
i2cdevWriteByte(I2Cx, devAddr, 0x61, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x62, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x64, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x65, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x66, 0xA0);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x22, 0x32);
i2cdevWriteByte(I2Cx, devAddr, 0x47, 0x14);
i2cdevWriteByte(I2Cx, devAddr, 0x49, 0xFF);
i2cdevWriteByte(I2Cx, devAddr, 0x4A, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x7A, 0x0A);
i2cdevWriteByte(I2Cx, devAddr, 0x7B, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x78, 0x21);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x23, 0x34);
i2cdevWriteByte(I2Cx, devAddr, 0x42, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x44, 0xFF);
i2cdevWriteByte(I2Cx, devAddr, 0x45, 0x26);
i2cdevWriteByte(I2Cx, devAddr, 0x46, 0x05);
i2cdevWriteByte(I2Cx, devAddr, 0x40, 0x40);
i2cdevWriteByte(I2Cx, devAddr, 0x0E, 0x06);
i2cdevWriteByte(I2Cx, devAddr, 0x20, 0x1A);
i2cdevWriteByte(I2Cx, devAddr, 0x43, 0x40);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x34, 0x03);
i2cdevWriteByte(I2Cx, devAddr, 0x35, 0x44);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x31, 0x04);
i2cdevWriteByte(I2Cx, devAddr, 0x4B, 0x09);
i2cdevWriteByte(I2Cx, devAddr, 0x4C, 0x05);
i2cdevWriteByte(I2Cx, devAddr, 0x4D, 0x04);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x44, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x45, 0x20);
i2cdevWriteByte(I2Cx, devAddr, 0x47, 0x08);
i2cdevWriteByte(I2Cx, devAddr, 0x48, 0x28);
i2cdevWriteByte(I2Cx, devAddr, 0x67, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x70, 0x04);
i2cdevWriteByte(I2Cx, devAddr, 0x71, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x72, 0xFE);
i2cdevWriteByte(I2Cx, devAddr, 0x76, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x77, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x0D, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x01, 0xF8);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x8E, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x00);
// -- VL53L0X_load_tuning_settings() end
// "Set interrupt config to new sample ready"
// -- VL53L0X_SetGpioConfig() begin
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
i2cdevWriteBit(I2Cx, devAddr, VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH, 4, 0); // active low
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
// -- VL53L0X_SetGpioConfig() end
measurement_timing_budget_us = vl53l0xGetMeasurementTimingBudget();
measurement_timing_budget_ms = (uint16_t)(measurement_timing_budget_us / 1000.0f);
// "Disable MSRC and TCC by default"
// MSRC = Minimum Signal Rate Check
// TCC = Target CentreCheck
// -- VL53L0X_SetSequenceStepEnable() begin
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
// -- VL53L0X_SetSequenceStepEnable() end
// "Recalculate timing budget"
vl53l0xSetMeasurementTimingBudget(measurement_timing_budget_us);
// VL53L0X_StaticInit() end
// VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
// -- VL53L0X_perform_vhv_calibration() begin
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x01);
if (!vl53l0xPerformSingleRefCalibration(0x40)) { DEBUG_PRINT("Failed VHV calibration\n"); return false; }
// -- VL53L0X_perform_vhv_calibration() end
// -- VL53L0X_perform_phase_calibration() begin
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
if (!vl53l0xPerformSingleRefCalibration(0x00)) { DEBUG_PRINT("Failed phase calibration\n"); return false; }
// -- VL53L0X_perform_phase_calibration() end
// "restore the previous Sequence Config"
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
// VL53L0X_PerformRefCalibration() end
return true;
}
// Set the return signal rate limit check value in units of MCPS (mega counts
// per second). "This represents the amplitude of the signal reflected from the
// target and detected by the device"; setting this limit presumably determines
// the minimum measurement necessary for the sensor to report a valid reading.
// Setting a lower limit increases the potential range of the sensor but also
// seems to increase the likelihood of getting an inaccurate reading because of
// unwanted reflections from objects other than the intended target.
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool vl53l0xSetSignalRateLimit(float limit_Mcps)
{
if (limit_Mcps < 0 || limit_Mcps > 511.99) { return false; }
// Q9.7 fixed point format (9 integer bits, 7 fractional bits)
uint16_t fixed_pt = limit_Mcps * (1 << 7);
return vl53l0xWriteReg16Bit(VL53L0X_RA_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, fixed_pt);
}
// Set the measurement timing budget in microseconds, which is the time allowed
// for one measurement; the ST API and this library take care of splitting the
// timing budget among the sub-steps in the ranging sequence. A longer timing
// budget allows for more accurate measurements. Increasing the budget by a
// factor of N decreases the range measurement standard deviation by a factor of
// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
bool vl53l0xSetMeasurementTimingBudget(uint32_t budget_us)
{
SequenceStepEnables enables;
SequenceStepTimeouts timeouts;
uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
uint16_t const EndOverhead = 960;
uint16_t const MsrcOverhead = 660;
uint16_t const TccOverhead = 590;
uint16_t const DssOverhead = 690;
uint16_t const PreRangeOverhead = 660;
uint16_t const FinalRangeOverhead = 550;
uint32_t const MinTimingBudget = 20000;
if (budget_us < MinTimingBudget) { return false; }
uint32_t used_budget_us = StartOverhead + EndOverhead;
vl53l0xGetSequenceStepEnables(&enables);
vl53l0xGetSequenceStepTimeouts(&enables, &timeouts);
if (enables.tcc)
{
used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
}
if (enables.dss)
{
used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
}
else if (enables.msrc)
{
used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
}
if (enables.pre_range)
{
used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
}
if (enables.final_range)
{
used_budget_us += FinalRangeOverhead;
// "Note that the final range timeout is determined by the timing
// budget and the sum of all other timeouts within the sequence.
// If there is no room for the final range timeout, then an error
// will be set. Otherwise the remaining time will be applied to
// the final range."
if (used_budget_us > budget_us)
{
// "Requested timeout too big."
return false;
}
uint32_t final_range_timeout_us = budget_us - used_budget_us;
// set_sequence_step_timeout() begin
// (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
// "For the final range timeout, the pre-range timeout
// must be added. To do this both final and pre-range
// timeouts must be expressed in macro periods MClks
// because they have different vcsel periods."
uint16_t final_range_timeout_mclks =
vl53l0xTimeoutMicrosecondsToMclks(final_range_timeout_us,
timeouts.final_range_vcsel_period_pclks);
if (enables.pre_range)
{
final_range_timeout_mclks += timeouts.pre_range_mclks;
}
uint16_t temp = vl53l0xEncodeTimeout(final_range_timeout_mclks);
vl53l0xWriteReg16Bit(VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, temp);
// set_sequence_step_timeout() end
measurement_timing_budget_us = budget_us; // store for internal reuse
measurement_timing_budget_ms = (uint16_t)(measurement_timing_budget_us / 1000.0f);
}
return true;
}
// Get the measurement timing budget in microseconds
// based on VL53L0X_get_measurement_timing_budget_micro_seconds()
// in us
uint32_t vl53l0xGetMeasurementTimingBudget(void)
{
SequenceStepEnables enables;
SequenceStepTimeouts timeouts;
uint16_t const StartOverhead = 1910; // note that this is different than the value in set_
uint16_t const EndOverhead = 960;
uint16_t const MsrcOverhead = 660;
uint16_t const TccOverhead = 590;
uint16_t const DssOverhead = 690;
uint16_t const PreRangeOverhead = 660;
uint16_t const FinalRangeOverhead = 550;
// "Start and end overhead times always present"
uint32_t budget_us = StartOverhead + EndOverhead;
vl53l0xGetSequenceStepEnables(&enables);
vl53l0xGetSequenceStepTimeouts(&enables, &timeouts);
if (enables.tcc)
{
budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
}
if (enables.dss)
{
budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
}
else if (enables.msrc)
{
budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
}
if (enables.pre_range)
{
budget_us += (timeouts.pre_range_us + PreRangeOverhead);
}
if (enables.final_range)
{
budget_us += (timeouts.final_range_us + FinalRangeOverhead);
}
measurement_timing_budget_us = budget_us; // store for internal reuse
measurement_timing_budget_ms = (uint16_t)(measurement_timing_budget_us / 1000.0f);
return budget_us;
}
// Set the VCSEL (vertical cavity surface emitting laser) pulse period for the
// given period type (pre-range or final range) to the given value in PCLKs.
// Longer periods seem to increase the potential range of the sensor.
// Valid values are (even numbers only):
// pre: 12 to 18 (initialized default: 14)
// final: 8 to 14 (initialized default: 10)
// based on VL53L0X_set_vcsel_pulse_period()
bool vl53l0xSetVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks)
{
uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
SequenceStepEnables enables;
SequenceStepTimeouts timeouts;
vl53l0xGetSequenceStepEnables(&enables);
vl53l0xGetSequenceStepTimeouts(&enables, &timeouts);
// "Apply specific settings for the requested clock period"
// "Re-calculate and apply timeouts, in macro periods"
// "When the VCSEL period for the pre or final range is changed,
// the corresponding timeout must be read from the device using
// the current VCSEL period, then the new VCSEL period can be
// applied. The timeout then must be written back to the device
// using the new VCSEL period.
//
// For the MSRC timeout, the same applies - this timeout being
// dependant on the pre-range vcsel period."
if (type == VcselPeriodPreRange)
{
// "Set phase check limits"
switch (period_pclks)
{
case 12:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
break;
case 14:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
break;
case 16:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
break;
case 18:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
break;
default:
// invalid period
return false;
}
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
// apply new VCSEL period
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
// update timeouts
// set_sequence_step_timeout() begin
// (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
uint16_t new_pre_range_timeout_mclks =
vl53l0xTimeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
uint16_t new_pre_range_timeout_encoded = vl53l0xEncodeTimeout(new_pre_range_timeout_mclks);
vl53l0xWriteReg16Bit(VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_pre_range_timeout_encoded);
// set_sequence_step_timeout() end
// set_sequence_step_timeout() begin
// (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
uint16_t new_msrc_timeout_mclks =
vl53l0xTimeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP,
(new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
// set_sequence_step_timeout() end
}
else if (type == VcselPeriodFinalRange)
{
switch (period_pclks)
{
case 8:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x30);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
break;
case 10:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
break;
case 12:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
break;
case 14:
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
break;
default:
// invalid period
return false;
}
// apply new VCSEL period
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
// update timeouts
// set_sequence_step_timeout() begin
// (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
// "For the final range timeout, the pre-range timeout
// must be added. To do this both final and pre-range
// timeouts must be expressed in macro periods MClks
// because they have different vcsel periods."
uint16_t new_final_range_timeout_mclks =
vl53l0xTimeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
if (enables.pre_range)
{
new_final_range_timeout_mclks += timeouts.pre_range_mclks;
}
uint16_t new_final_range_timeout_encoded = vl53l0xEncodeTimeout(new_final_range_timeout_mclks);
vl53l0xWriteReg16Bit(VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_final_range_timeout_encoded);
// set_sequence_step_timeout end
}
else
{
// invalid type
return false;
}
// "Finally, the timing budget must be re-applied"
vl53l0xSetMeasurementTimingBudget(measurement_timing_budget_us);
// "Perform the phase calibration. This is needed after changing on vcsel period."
// VL53L0X_perform_phase_calibration() begin
uint8_t sequence_config = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
bool ret = vl53l0xPerformSingleRefCalibration(0x0);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, sequence_config);
// VL53L0X_perform_phase_calibration() end
return ret;
}
// Get the VCSEL pulse period in PCLKs for the given period type.
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t vl53l0xGetVcselPulsePeriod(vcselPeriodType type)
{
if (type == VcselPeriodPreRange)
{
uint8_t temp = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, &temp);
return decodeVcselPeriod(temp);
}
else if (type == VcselPeriodFinalRange)
{
uint8_t temp = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &temp);
return decodeVcselPeriod(temp);
}
else { return 255; }
}
// Start continuous ranging measurements. If period_ms (optional) is 0 or not
// given, continuous back-to-back mode is used (the sensor takes measurements as
// often as possible); otherwise, continuous timed mode is used, with the given
// inter-measurement period in milliseconds determining how often the sensor
// takes a measurement.
// based on VL53L0X_StartMeasurement()
void vl53l0xStartContinuous(uint32_t period_ms)
{
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x91, stop_variable);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x00);
if (period_ms != 0)
{
// continuous timed mode
// VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
uint16_t osc_calibrate_val = vl53l0xReadReg16Bit(VL53L0X_RA_OSC_CALIBRATE_VAL);
if (osc_calibrate_val != 0)
{
period_ms *= osc_calibrate_val;
}
vl53l0xWriteReg32Bit(VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
// VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
}
else
{
// continuous back-to-back mode
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
}
}
// Stop continuous measurements
// based on VL53L0X_StopMeasurement()
void vl53l0xStopContinuous(void)
{
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x91, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
}
// Returns a range reading in millimeters when continuous mode is active
// (readRangeSingleMillimeters() also calls this function after starting a
// single-shot range measurement)
uint16_t vl53l0xReadRangeContinuousMillimeters(void)
{
startTimeout();
uint8_t val = 0;
while ((val & 0x07) == 0)
{
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val);
if (checkTimeoutExpired())
{
did_timeout = true;
return 65535;
}
}
// assumptions: Linearity Corrective Gain is 1000 (default);
// fractional ranging is not enabled
uint16_t range = vl53l0xReadReg16Bit(VL53L0X_RA_RESULT_RANGE_STATUS + 10);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
return range;
}
// Performs a single-shot range measurement and returns the reading in
// millimeters
// based on VL53L0X_PerformSingleRangingMeasurement()
uint16_t vl53l0xReadRangeSingleMillimeters(void)
{
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x91, stop_variable);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x00);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x01);
// "Wait until start bit has been cleared"
startTimeout();
uint8_t val = 0x01;
while (val & 0x01)
{
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, &val);
if (checkTimeoutExpired())
{
did_timeout = true;
return 65535;
}
}
return vl53l0xReadRangeContinuousMillimeters();
}
// Get reference SPAD (single photon avalanche diode) count and type
// based on VL53L0X_get_info_from_device(),
// but only gets reference SPAD count and type
bool vl53l0xGetSpadInfo(uint8_t * count, bool * type_is_aperture)
{
uint8_t tmp;
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x06);
i2cdevWriteBit(I2Cx, devAddr, 0x83, 2, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x07);
i2cdevWriteByte(I2Cx, devAddr, 0x81, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x94, 0x6b);
i2cdevWriteByte(I2Cx, devAddr, 0x83, 0x00);
startTimeout();
uint8_t val = 0x00;
while (val == 0x00) {
i2cdevReadByte(I2Cx, devAddr, 0x83, &val);
if (checkTimeoutExpired()) { return false; }
};
i2cdevWriteByte(I2Cx, devAddr, 0x83, 0x01);
i2cdevReadByte(I2Cx, devAddr, 0x92, &tmp);
*count = tmp & 0x7f;
*type_is_aperture = (tmp >> 7) & 0x01;
i2cdevWriteByte(I2Cx, devAddr, 0x81, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x06);
i2cdevWriteBit(I2Cx, devAddr, 0x83, 2, 0);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0x00, 0x01);
i2cdevWriteByte(I2Cx, devAddr, 0xFF, 0x00);
i2cdevWriteByte(I2Cx, devAddr, 0x80, 0x00);
return true;
}
// Get sequence step enables
// based on VL53L0X_GetSequenceStepEnables()
void vl53l0xGetSequenceStepEnables(SequenceStepEnables * enables)
{
uint8_t sequence_config = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
enables->tcc = (sequence_config >> 4) & 0x1;
enables->dss = (sequence_config >> 3) & 0x1;
enables->msrc = (sequence_config >> 2) & 0x1;
enables->pre_range = (sequence_config >> 6) & 0x1;
enables->final_range = (sequence_config >> 7) & 0x1;
}
// Get sequence step timeouts
// based on get_sequence_step_timeout(),
// but gets all timeouts instead of just the requested one, and also stores
// intermediate values
void vl53l0xGetSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
{
timeouts->pre_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(VcselPeriodPreRange);
uint8_t temp = 0;
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP, &temp);
timeouts->msrc_dss_tcc_mclks = temp + 1;
timeouts->msrc_dss_tcc_us =
vl53l0xTimeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
timeouts->pre_range_vcsel_period_pclks);
uint16_t pre_range_encoded = vl53l0xReadReg16Bit(VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
timeouts->pre_range_mclks = vl53l0xDecodeTimeout(pre_range_encoded);
timeouts->pre_range_us =
vl53l0xTimeoutMclksToMicroseconds(timeouts->pre_range_mclks,
timeouts->pre_range_vcsel_period_pclks);
timeouts->final_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(VcselPeriodFinalRange);
uint16_t final_range_encoded = vl53l0xReadReg16Bit(VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
timeouts->final_range_mclks = vl53l0xDecodeTimeout(final_range_encoded);
if (enables->pre_range)
{
timeouts->final_range_mclks -= timeouts->pre_range_mclks;
}
timeouts->final_range_us =
vl53l0xTimeoutMclksToMicroseconds(timeouts->final_range_mclks,
timeouts->final_range_vcsel_period_pclks);
}
// Decode sequence step timeout in MCLKs from register value
// based on VL53L0X_decode_timeout()
// Note: the original function returned a uint32_t, but the return value is
// always stored in a uint16_t.
uint16_t vl53l0xDecodeTimeout(uint16_t reg_val)
{
// format: "(LSByte * 2^MSByte) + 1"
return (uint16_t)((reg_val & 0x00FF) <<
(uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
}
// Encode sequence step timeout register value from timeout in MCLKs
// based on VL53L0X_encode_timeout()
// Note: the original function took a uint16_t, but the argument passed to it
// is always a uint16_t.
uint16_t vl53l0xEncodeTimeout(uint16_t timeout_mclks)
{
// format: "(LSByte * 2^MSByte) + 1"
uint32_t ls_byte = 0;
uint16_t ms_byte = 0;
if (timeout_mclks > 0)
{
ls_byte = timeout_mclks - 1;
while ((ls_byte & 0xFFFFFF00LU) > 0)
{
ls_byte >>= 1;
ms_byte++;
}
return (ms_byte << 8) | (ls_byte & 0xFF);
}
else { return 0; }
}
// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
// based on VL53L0X_calc_timeout_us()
uint32_t vl53l0xTimeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
}
// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
// based on VL53L0X_calc_timeout_mclks()
uint32_t vl53l0xTimeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
}
// based on VL53L0X_perform_single_ref_calibration()
bool vl53l0xPerformSingleRefCalibration(uint8_t vhv_init_byte)
{
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
startTimeout();
uint8_t temp = 0x00;
while ((temp & 0x07) == 0)
{
i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &temp);
if (checkTimeoutExpired()) { return false; }
}
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
i2cdevWriteByte(I2Cx, devAddr, VL53L0X_RA_SYSRANGE_START, 0x00);
return true;
}
uint16_t vl53l0xReadReg16Bit(uint8_t reg)
{
uint8_t buffer[2] = {};
i2cdevRead(I2Cx, devAddr, reg, 2, (uint8_t *)&buffer);
return ((uint16_t)(buffer[0]) << 8) | buffer[1];
}
bool vl53l0xWriteReg16Bit(uint8_t reg, uint16_t val)
{
uint8_t buffer[2] = {};
buffer[0] = ((val >> 8) & 0xFF);
buffer[1] = ((val ) & 0xFF);
return i2cdevWrite(I2Cx, devAddr, reg, 2, (uint8_t *)&buffer);
}
bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val)
{
uint8_t buffer[4] = {};
buffer[0] = ((val >> 24) & 0xFF);
buffer[1] = ((val >> 16) & 0xFF);
buffer[2] = ((val >> 8) & 0xFF);
buffer[3] = ((val ) & 0xFF);
return i2cdevWrite(I2Cx, devAddr, reg, 4, (uint8_t *)&buffer);
}
// TODO: Decide on vid:pid and set the used pins
static const DeckDriver vl53l0x_deck = {
.vid = 0, // Changed this from 0
.pid = 0, // Changed this from 0
.name = "vl53l0x",
.usedGpio = 0,
.init = vl53l0xInit,
.test = vl53l0xTest,
};
DECK_DRIVER(vl53l0x_deck);
LOG_GROUP_START(range)
LOG_ADD(LOG_UINT16, range, &range_last)
LOG_GROUP_STOP(range)