Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Master.crtp position update #147

Merged
merged 17 commits into from
Sep 22, 2016
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ bin/*
tools/make/config.mk
cflie.*
version.c
tags

/cf2.*
/cf1.*
Expand Down
3 changes: 2 additions & 1 deletion Makefile
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,8 @@ PROJ_OBJ_CF1 += sound_cf1.o sensors_stock.o
PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o

# Stabilizer modules
PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o
PROJ_OBJ += commander.o ext_position.o
PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o
PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o
PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o
Expand Down
2 changes: 1 addition & 1 deletion src/hal/src/eskylink.c
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ static void eskylinkDecode(char* packet)
if (thrust>(2*PPM_RANGE)) thrust = 2*PPM_RANGE;
thrust *= 55000/(2*PPM_RANGE);

crtpPacket.port = CRTP_PORT_COMMANDER;
crtpPacket.port = CRTP_PORT_SETPOINT;
memcpy(&crtpPacket.data[0], (char*)&roll, 4);
memcpy(&crtpPacket.data[4], (char*)&pitch, 4);
memcpy(&crtpPacket.data[8], (char*)&yaw, 4);
Expand Down
3 changes: 2 additions & 1 deletion src/modules/interface/crtp.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,10 @@
typedef enum {
CRTP_PORT_CONSOLE = 0x00,
CRTP_PORT_PARAM = 0x02,
CRTP_PORT_COMMANDER = 0x03,
CRTP_PORT_SETPOINT = 0x03,
CRTP_PORT_MEM = 0x04,
CRTP_PORT_LOG = 0x05,
CRTP_PORT_POSITION = 0x06,
CRTP_PORT_PLATFORM = 0x0D,
CRTP_PORT_LINK = 0x0F,
} CRTPPort;
Expand Down
47 changes: 47 additions & 0 deletions src/modules/interface/ext_position.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
/**
* || ____ _ __ ______
* +------+ / __ )(_) /_/ ____/_________ _____ ___
* | 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/>.
*
*/

#ifndef _EXT_POSITION_H_
#define _EXT_POSITION_H_

#include "stabilizer_types.h"

/**
* CRTP external position data struct
*/
struct CrtpExtPosition
{
float x; // in m
float y; // in m
float z; // in m
} __attribute__((packed));

// Set up the callback for the CRTP_PORT_POSITION
void extPositionInit(void);

// Get the current position from the cache
bool getExtPosition(state_t *state);

#endif /* _EXT_POSITION_H_ */
7 changes: 6 additions & 1 deletion src/modules/src/commander.c
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,11 @@
#include "commander.h"
#include "crtp.h"
#include "configblock.h"
#include "log.h"
#include "param.h"
#include "num.h"
#include "debug.h"
#include "ext_position.h"

#define MIN_THRUST 1000
#define MAX_THRUST 60000
Expand All @@ -47,6 +50,7 @@ typedef struct
uint32_t timestamp; // FreeRTOS ticks
} CommanderCache;


/**
* Stabilization modes for Roll, Pitch, Yaw.
*/
Expand Down Expand Up @@ -256,7 +260,8 @@ void commanderInit(void)
}

crtpInit();
crtpRegisterPortCB(CRTP_PORT_COMMANDER, commanderCrtpCB);
crtpRegisterPortCB(CRTP_PORT_SETPOINT, commanderCrtpCB);
extPositionInit(); // Set callback for CRTP_PORT_POSITION

activeCache = &crtpCache;
lastUpdate = xTaskGetTickCount();
Expand Down
95 changes: 95 additions & 0 deletions src/modules/src/ext_position.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie 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/>.
*
*
*/

#include "FreeRTOS.h"
#include "task.h"

#include "ext_position.h"
#include "crtp.h"
#include "log.h"

#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
#endif

/**
* Position data cache
*/
typedef struct
{
struct CrtpExtPosition targetVal[2];
bool activeSide;
uint32_t timestamp; // FreeRTOS ticks
} ExtPositionCache;

// Struct for logging position information
static positionMeasurement_t ext_pos;
static ExtPositionCache crtpExtPosCache;
static void extPositionCrtpCB(CRTPPacket* pk);

static bool isInit = false;

void extPositionInit()
{
if (isInit) {
return;
}

crtpRegisterPortCB(CRTP_PORT_POSITION, extPositionCrtpCB);
isInit = true;
}

static void extPositionCrtpCB(CRTPPacket* pk)
{
crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = *((struct CrtpExtPosition*)pk->data);
crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide;
crtpExtPosCache.timestamp = xTaskGetTickCount();
}


bool getExtPosition(state_t *state)
{
// Only use position information if it's valid and recent
if ((xTaskGetTickCount() - crtpExtPosCache.timestamp) < M2T(5)) {
// Get the updated position from the mocap
ext_pos.x = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].x;
ext_pos.y = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].y;
ext_pos.z = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].z;
ext_pos.stdDev = 0.01;
#ifdef ESTIMATOR_TYPE_kalman
stateEstimatorEnqueuePosition(&ext_pos);
#endif
return true;
}
return false;
}

LOG_GROUP_START(ext_pos)
LOG_ADD(LOG_FLOAT, X, &ext_pos.x)
LOG_ADD(LOG_FLOAT, Y, &ext_pos.y)
LOG_ADD(LOG_FLOAT, Z, &ext_pos.z)
LOG_GROUP_STOP(ext_pos)

2 changes: 2 additions & 0 deletions src/modules/src/stabilizer.c
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include "sensors.h"
#include "commander.h"
#include "ext_position.h"
#include "sitaw.h"
#include "controller.h"
#include "power_distribution.h"
Expand Down Expand Up @@ -110,6 +111,7 @@ static void stabilizerTask(void* param)
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));

getExtPosition(&state);
#ifdef ESTIMATOR_TYPE_kalman
stateEstimatorUpdate(&state, &sensorData, &control);
#else
Expand Down
2 changes: 1 addition & 1 deletion vendor/libdw1000