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
Expand all 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 @ 7a8d0f72
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
#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 @ 7a8d0f72
#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 @ 7a8d0f72
#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 @ 7a8d0f72
#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 @ 7a8d0f72
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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 @ 7a8d0f72
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 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/rzr.c deleted 100755 → 0
View file @ 7a8d0f72
This diff is collapsed.