Skip to content

Commit

Permalink
#102 Moved situation awareness out of stabilizer
Browse files Browse the repository at this point in the history
  • Loading branch information
@ataffanel
ataffanel committed Apr 26, 2016
1 parent 2b230ee commit 8ba8d09
Show file tree
Hide file tree
Showing 3 changed files with 91 additions and 118 deletions.
5 changes: 4 additions & 1 deletion src/modules/interface/sitaw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,14 +30,17 @@
#include <stdint.h>
#include <stdbool.h>

#include "stabilizer_types.h"

bool sitAwFFTest(float accWZ, float accMag);
bool sitAwFFDetected(void);
bool sitAwARTest(float accX, float accY, float accZ);
bool sitAwARDetected(void);
bool sitAwTuTest(float eulerRollActual, float eulerPitchActual);
bool sitAwTuDetected(void);
void sitAwInit(void);

void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
const state_t *state);
/* Enable the situation awareness framework. */
//#define SITAW_ENABLED

Expand Down
57 changes: 57 additions & 0 deletions src/modules/src/sitaw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include "param.h"
#include "trigger.h"
#include "sitaw.h"
#include "commander.h"

/* Trigger object used to detect Free Fall situation. */
static trigger_t sitAwFFAccWZ;
Expand Down Expand Up @@ -99,6 +100,62 @@ void sitAwFFInit(void)
triggerActivate(&sitAwFFAccWZ, true);
}


static void sitAwPostStateUpdateCallOut(const sensorData_t *sensorData,
const state_t *state)
{
/* Code that shall run AFTER each attitude update, should be placed here. */

#if defined(SITAW_ENABLED)
float accMAG = (sensorData->acc.x*sensorData->acc.x) +
(sensorData->acc.y*sensorData->acc.y) +
(sensorData->acc.z*sensorData->acc.z);

/* Test values for Free Fall detection. */
sitAwFFTest(state->acc.z, accMAG);

/* Test values for Tumbled detection. */
sitAwTuTest(state->attitude.roll, state->attitude.pitch);

/* Test values for At Rest detection. */
sitAwARTest(sensorData->acc.x, sensorData->acc.y, sensorData->acc.z);
#endif
}

static void sitAwPreThrustUpdateCallOut(setpoint_t *setpoint)
{
/* Code that shall run BEFORE each thrust distribution update, should be placed here. */

#if defined(SITAW_ENABLED)
if(sitAwTuDetected()) {
/* Kill the thrust to the motors if a Tumbled situation is detected. */
setpoint->thrust = 0;
}

/* Force altHold mode if free fall is detected.
FIXME: Needs a flying/landing state (as soon as althold is enabled,
we are not freefalling anymore)
*/
if(sitAwFFDetected() && !sitAwTuDetected()) {
setpoint->mode.z = modeVelocity;
setpoint->velocity.z = 0;
}
#endif
}

/**
* Update setpoint according to current situation
*
* Called by the stabilizer after state and setpoint update. This function
* should update the setpoint accordig to the current state situation
*/
void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
const state_t *state)
{
sitAwPostStateUpdateCallOut(sensorData, state);
sitAwPreThrustUpdateCallOut(setpoint);
}

/**
* Test values for a Free Fall situation.
*
Expand Down
147 changes: 30 additions & 117 deletions src/modules/src/stabilizer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,15 +30,13 @@

#include "config.h"
#include "system.h"
#include "pm.h"
#include "stabilizer.h"
#include "commander.h"
#include "attitude_controller.h"
#include "sensfusion6.h"
#include "imu.h"
#include "motors.h"
#include "log.h"
#include "pid.h"
#include "param.h"
#include "sitaw.h"
#ifdef PLATFORM_CF1
Expand All @@ -47,70 +45,18 @@
#include "lps25h.h"
#endif
#include "num.h"
#include "position_estimator.h"
#include "position_controller.h"
#include "altitude_hold.h"


/**
* Defines in what divided update rate should the attitude
* control loop run relative the rate control loop.
*/
#define ATTITUDE_UPDATE_RATE_DIVIDER 2
#define ATTITUDE_UPDATE_DT (float)(1.0 / (IMU_UPDATE_FREQ / ATTITUDE_UPDATE_RATE_DIVIDER)) // 250hz

#define ALTHOLD_UPDATE_RATE_DIVIDER 5
#define ALTHOLD_UPDATE_DT (float)(1.0 / (IMU_UPDATE_FREQ / ALTHOLD_UPDATE_RATE_DIVIDER)) // 100hz

// // Barometer/ Altitude hold stuff
// static float accWZ = 0.0; // Acceleration Without gravity along Z axis (G).
// static float accMAG = 0.0; // Acceleration magnitude
//
// static Axis3f gyro; // Gyro axis data in deg/s
// static Axis3f acc; // Accelerometer axis data in mG
// static Axis3f mag; // Magnetometer axis data in testla
//
// static float eulerRollActual; // Measured roll angle in deg
// static float eulerPitchActual; // Measured pitch angle in deg
// static float eulerYawActual; // Measured yaw angle in deg
// static float eulerRollDesired; // Desired roll angle in deg
// static float eulerPitchDesired; // Desired ptich angle in deg
// static float eulerYawDesired; // Desired yaw angle in deg
// static float rollRateDesired; // Desired roll rate in deg/s
// static float pitchRateDesired; // Desired pitch rate in deg/s
// static float yawRateDesired; // Desired yaw rate in deg/s
//
//
// static float carefreeFrontAngle = 0; // carefree front angle that is set
//
// uint16_t actuatorThrust; // Actuator output for thrust base
// int16_t actuatorRoll; // Actuator output roll compensation
// int16_t actuatorPitch; // Actuator output pitch compensation
// int16_t actuatorYaw; // Actuator output yaw compensation
//
uint32_t motorPowerM1; // Motor 1 power output (16bit value used: 0 - 65535)
uint32_t motorPowerM2; // Motor 2 power output (16bit value used: 0 - 65535)
uint32_t motorPowerM3; // Motor 3 power output (16bit value used: 0 - 65535)
uint32_t motorPowerM4; // Motor 4 power output (16bit value used: 0 - 65535)
static struct {
uint16_t m1;
uint16_t m2;
uint16_t m3;
uint16_t m4;
} motorPower;

static bool isInit;


// static void stabilizerRotateYaw(float yawRad);
// static void stabilizerRotateYawCarefree(bool reset);
// static void stabilizerYawModeUpdate(void);
// static void distributePower(const uint16_t thrust, const int16_t roll,
// const int16_t pitch, const int16_t yaw);
static uint16_t limitThrust(int32_t value);
static void stabilizerTask(void* param);
// static void readBarometerData(float* pressure, float* temperature, float* asl);

// Baro variables
// static float temperature; // temp from barometer in celcius
// static float pressure; // pressure from barometer in bar
// static float asl; // raw Altitude over Sea Level from pressure sensor, in meters. Has an offset.
// static estimate_t estimatedPosition;


void stabilizerInit(void)
{
Expand Down Expand Up @@ -147,43 +93,6 @@ bool stabilizerTest(void)
return pass;
}

// static void stabilizerPostAttitudeUpdateCallOut(void)
// {
// /* Code that shall run AFTER each attitude update, should be placed here. */
//
// #if defined(SITAW_ENABLED)
// /* Test values for Free Fall detection. */
// sitAwFFTest(accWZ, accMAG);
//
// /* Test values for Tumbled detection. */
// sitAwTuTest(eulerRollActual, eulerPitchActual);
//
// /* Test values for At Rest detection. */
// sitAwARTest(acc.x, acc.y, acc.z);
//
// /* Enable altHold mode if free fall is detected. */
// if(sitAwFFDetected() && !sitAwTuDetected()) {
// commanderSetAltHoldMode(true);
// }
//
// /* Disable altHold mode if a Tumbled situation is detected. */
// if(sitAwTuDetected()) {
// commanderSetAltHoldMode(false);
// }
// #endif
// }
//
// static void stabilizerPreThrustUpdateCallOut(void)
// {
// /* Code that shall run BEFORE each thrust distribution update, should be placed here. */
//
// #if defined(SITAW_ENABLED)
// if(sitAwTuDetected()) {
// /* Kill the thrust to the motors if a Tumbled situation is detected. */
// actuatorThrust = 0;
// }
// #endif
// }
//
// static void stabilizerTask(void* param)
// {
Expand Down Expand Up @@ -408,43 +317,45 @@ static void readBarometerData(baro_t *baro) {
#endif
}

static void acquireSensors(sensorData_t *sensors)
static bool acquireSensors(sensorData_t *sensors)
{
if (RATE_SKIP_500HZ()) {
return;
return imu6IsCalibrated();
}

imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag);
if (imuHasBarometer()) {
readBarometerData(&sensors->baro);
}
// Get the position

return imu6IsCalibrated();
}

void distributePower(const control_t *control)
{
#ifdef QUAD_FORMATION_X
int16_t r = control->roll / 2.0f;
int16_t p = control->pitch / 2.0f;
motorPowerM1 = limitThrust(control->thrust - r + p + control->yaw);
motorPowerM2 = limitThrust(control->thrust - r - p - control->yaw);
motorPowerM3 = limitThrust(control->thrust + r - p + control->yaw);
motorPowerM4 = limitThrust(control->thrust + r + p - control->yaw);
motorPower.m1 = limitThrust(control->thrust - r + p + control->yaw);
motorPower.m2 = limitThrust(control->thrust - r - p - control->yaw);
motorPower.m3 = limitThrust(control->thrust + r - p + control->yaw);
motorPower.m4 = limitThrust(control->thrust + r + p - control->yaw);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(control->thrust + control->pitch +
motorPower.m1 = limitThrust(control->thrust + control->pitch +
control->yaw);
motorPowerM2 = limitThrust(control->thrust - control->roll -
motorPower.m2 = limitThrust(control->thrust - control->roll -
control->yaw);
motorPowerM3 = limitThrust(control->thrust - control->pitch +
motorPower.m3 = limitThrust(control->thrust - control->pitch +
control->yaw);
motorPowerM4 = limitThrust(control->thrust + control->roll -
motorPower.m4 = limitThrust(control->thrust + control->roll -
control->yaw);
#endif

motorsSetRatio(MOTOR_M1, motorPowerM1);
motorsSetRatio(MOTOR_M2, motorPowerM2);
motorsSetRatio(MOTOR_M3, motorPowerM3);
motorsSetRatio(MOTOR_M4, motorPowerM4);
motorsSetRatio(MOTOR_M1, motorPower.m1);
motorsSetRatio(MOTOR_M2, motorPower.m2);
motorsSetRatio(MOTOR_M3, motorPower.m3);
motorsSetRatio(MOTOR_M4, motorPower.m4);
}

// State variables for the stabilizer
Expand All @@ -470,11 +381,13 @@ static void stabilizerTask(void* param)
{
vTaskDelayUntil(&lastWakeTime, F2T(1000));

acquireSensors(&sensorData);
if (imu6IsCalibrated())
if (acquireSensors(&sensorData))
{
stateEstimator(&state, &sensorData);
commanderGetSetpoint(&setpoint);

sitAwUpdateSetpoint(&setpoint, &sensorData, &state);

stateController(&control, &sensorData, &state, &setpoint);
distributePower(&control);
}
Expand Down Expand Up @@ -518,10 +431,10 @@ LOG_ADD(LOG_FLOAT, z, &sensorData.mag.z)
LOG_GROUP_STOP(mag)

LOG_GROUP_START(motor)
LOG_ADD(LOG_INT32, m4, &motorPowerM4)
LOG_ADD(LOG_INT32, m1, &motorPowerM1)
LOG_ADD(LOG_INT32, m2, &motorPowerM2)
LOG_ADD(LOG_INT32, m3, &motorPowerM3)
LOG_ADD(LOG_INT32, m4, &motorPower.m4)
LOG_ADD(LOG_INT32, m1, &motorPower.m1)
LOG_ADD(LOG_INT32, m2, &motorPower.m2)
LOG_ADD(LOG_INT32, m3, &motorPower.m3)
LOG_GROUP_STOP(motor)

LOG_GROUP_START(controller)
Expand Down

0 comments on commit 8ba8d09

Please sign in to comment.