Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2164 → Rev 2189

/branches/dongfang_FC_rewrite/dynamic_calibration_scrap.txt
File deleted
/branches/dongfang_FC_rewrite/readme.txt
File deleted
\ No newline at end of file
/branches/dongfang_FC_rewrite/commands.txt
File deleted
/branches/dongfang_FC_rewrite/sod2
File deleted
/branches/dongfang_FC_rewrite/attitudeControl.c
File deleted
/branches/dongfang_FC_rewrite/attitudeControl.h
File deleted
/branches/dongfang_FC_rewrite/can_I_get_your_contacts.txt
File deleted
/branches/dongfang_FC_rewrite/displays.txt
File deleted
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.h
File deleted
/branches/dongfang_FC_rewrite/fc-0.74_bugs.txt
File deleted
/branches/dongfang_FC_rewrite/dongfangMath.c
File deleted
/branches/dongfang_FC_rewrite/attitude.c
File deleted
/branches/dongfang_FC_rewrite/dongfangMath.h
File deleted
/branches/dongfang_FC_rewrite/printf_P.c
File deleted
/branches/dongfang_FC_rewrite/yaw-analysis.txt
File deleted
/branches/dongfang_FC_rewrite/printf_P.h
File deleted
/branches/dongfang_FC_rewrite/spi.c
File deleted
/branches/dongfang_FC_rewrite/spi.h
File deleted
/branches/dongfang_FC_rewrite/menu.c
File deleted
/branches/dongfang_FC_rewrite/menu.h
File deleted
/branches/dongfang_FC_rewrite/navi-lang.txt
File deleted
\ No newline at end of file
/branches/dongfang_FC_rewrite/encoding-tester.txt
File deleted
/branches/dongfang_FC_rewrite/bugs.txt
File deleted
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
9,10 → 9,8
}
 
void gyro_setDefaultParameters(void) {
IMUConfig.gyroQuadrant = 4;
IMUConfig.accQuadrant = 4;
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_XY;
staticParams.gyroD = 5;
IMUConfig.driftCompDivider = 2;
IMUConfig.driftCompLimit = 3;
IMUConfig.zerothOrderCorrection = 3;
IMUConfig.imuReversedFlags = IMU_REVERSE_ACCEL_Z;
// staticParams.gyroD = 5;
}
/branches/dongfang_FC_rewrite/AP_AHRS.h
0,0 → 1,97
#ifndef AP_AHRS_H
#define AP_AHRS_H
/*
AHRS (Attitude Heading Reference System) interface for ArduPilot
 
This library 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 2.1 of the License, or (at your option) any later version.
*/
 
#include "AP_Math.h"
#include <inttypes.h>
#include "AP_Compass.h"
#include "AP_GPS.h"
 
class AP_AHRS {
public:
AP_AHRS(/*IMU *imu, */ GPS *&gps):
_gps(gps)
{
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
// prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = ToRad(3.0/60);
}
 
// Accessors
void set_centripetal(bool b) { _centripetal = b; }
bool get_centripetal(void) { return _centripetal; }
void set_compass(Compass *compass) { _compass = compass; }
 
// Methods
virtual void update(int16_t attitude[3], float delta_t) = 0;
 
// Euler angles (radians)
float roll;
float pitch;
float yaw;
 
// return a smoothed and corrected gyro vector
virtual Vector3f get_gyro(void);
 
// return the current estimate of the gyro drift
virtual Vector3f get_gyro_drift(void) = 0;
 
// reset the current attitude, used on new IMU calibration
virtual void reset(bool recover_eulers=false) = 0;
 
// how often our attitude representation has gone out of range
uint8_t renorm_range_count;
 
// how often our attitude representation has blown up completely
uint8_t renorm_blowup_count;
 
// return the average size of the roll/pitch error estimate
// since last call
virtual float get_error_rp(void)= 0;
 
// return the average size of the yaw error estimate
// since last call
virtual float get_error_yaw(void) = 0;
 
// return a DCM rotation matrix representing our current
// attitude
virtual Matrix3f get_dcm_matrix(void) = 0;
 
protected:
// pointer to compass object, if enabled
Compass * _compass;
 
// time in microseconds of last compass update
uint32_t _compass_last_update;
 
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps;
// IMU *_imu;
 
// true if we are doing centripetal acceleration correction
bool _centripetal;
 
// the limit of the gyro drift claimed by the sensors, in
// radians/s/s
float _gyro_drift_limit;
 
// acceleration due to gravity in m/s/s
static const float _gravity = 9.80665;
};
 
#include "AP_AHRS_DCM.h"
//#include <AP_AHRS_Quaternion.h>
//#include <AP_AHRS_HIL.h>
 
#endif // AP_AHRS_H
/branches/dongfang_FC_rewrite/AP_AHRS_DCM.cpp
0,0 → 1,537
/*
APM_AHRS_DCM.cpp
 
AHRS system using DCM matrices
 
Based on DCM code by Doug Weibel, Jordi Mu�oz and Jose Julio. DIYDrones.com
 
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
 
This library 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 2.1
of the License, or (at your option) any later version.
*/
#include "AP_AHRS.h"
#include "output.h"
#include "profiler.h"
#include "analog.h"
#include "debug.h"
#include <stdio.h>
 
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
#define GPS_SPEED_MIN 300
 
// this is the speed in cm/s at which we stop using drift correction
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
 
// run a full DCM update round
void
AP_AHRS_DCM::update(int16_t attitude[3], float delta_t)
{
// Get current values for gyros
_gyro_vector = gyro_attitude;
_accel_vector = accel;
 
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
 
// Normalize the DCM matrix
normalize();
 
// Perform drift correction
//setCurrentProfiledActivity(DRIFT_CORRECTION);
drift_correction(delta_t);
 
// paranoid check for bad values in the DCM matrix
//setCurrentProfiledActivity(CHECK_MATRIX);
check_matrix();
 
// Calculate pitch, roll, yaw for stabilization and navigation
//setCurrentProfiledActivity(EULER_ANGLES);
euler_angles();
 
//setCurrentProfiledActivity(ANGLESOUTPUT);
attitude[0] = roll * INT16DEG_PI_FACTOR;
attitude[1] = pitch* INT16DEG_PI_FACTOR;
attitude[2] = yaw * INT16DEG_PI_FACTOR;
 
// Just for info:
int16_t sensor = degrees(roll) * 10;
debugOut.analog[0] = sensor;
 
sensor = degrees(pitch) * 10;
debugOut.analog[1] = sensor;
sensor = degrees(yaw) * 10;
if (sensor < 0)
sensor += 3600;
debugOut.analog[2] = sensor;
}
 
// update the DCM matrix using only the gyros
void
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
//setCurrentProfiledActivity(MATRIX_UPDATE1);
 
// _omega_integ_corr is used for _centripetal correction
// (theoretically better than _omega)
_omega_integ_corr = _gyro_vector + _omega_I;
 
// Equation 16, adding proportional and integral correction terms
_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
 
// this is a replacement of the DCM matrix multiply (equation
// 17), with known zero elements removed and the matrix
// operations inlined. This runs much faster than the original
// version of this code, as the compiler was doing a terrible
// job of realising that so many of the factors were in common
// or zero. It also uses much less stack, as we no longer need
// two additional local matrices
 
Vector3f r = _omega * _G_Dt;
 
//setCurrentProfiledActivity(MATRIX_UPDATE2);
 
_dcm_matrix.rotate(r);
}
 
 
// adjust an accelerometer vector for known acceleration forces
void
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
{
float veloc;
// compensate for linear acceleration. This makes a
// surprisingly large difference in the pitch estimate when
// turning, plus on takeoff and landing
//float acceleration = _gps->acceleration();
//accel.x -= acceleration;
 
// compensate for centripetal acceleration
veloc = 0; //_gps->ground_speed * 0.01;
 
// We are working with a modified version of equation 26 as
// our IMU object reports acceleration in the positive axis
// direction as positive
 
// Equation 26 broken up into separate pieces
accel.y -= _omega_integ_corr.z * veloc;
accel.z += _omega_integ_corr.y * veloc;
}
 
/*
reset the DCM matrix and omega. Used on ground start, and on
extreme errors in the matrix
*/
void
AP_AHRS_DCM::reset(bool recover_eulers)
{
if (_compass != NULL) {
_compass->null_offsets_disable();
}
 
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega_integ_corr.zero();
_omega.zero();
 
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// otherwise make it flat
_dcm_matrix.from_euler(0, 0, 0);
}
 
if (_compass != NULL) {
_compass->null_offsets_enable(); // This call is needed to restart the nulling
// Otherwise the reset in the DCM matrix can mess up
// the nulling
}
}
 
/*
check the DCM matrix for pathological values
*/
void
AP_AHRS_DCM::check_matrix(void)
{
if (_dcm_matrix.is_nan()) {
//Serial.printf("ERROR: DCM matrix NAN\n");
printf("ERROR: DCM matrix NAN\n");
renorm_blowup_count++;
reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
// the pitch calculation via asin(). These NaN values can
// feed back into the rest of the DCM matrix via the
// error_course value.
if (!(_dcm_matrix.c.x < 1.0 &&
_dcm_matrix.c.x > -1.0)) {
// We have an invalid matrix. Force a normalisation.
renorm_range_count++;
normalize();
 
if (_dcm_matrix.is_nan() ||
fabs(_dcm_matrix.c.x) > 10) {
// normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
// _dcm_matrix.c.x);
printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", (double)_dcm_matrix.c.x);
renorm_blowup_count++;
reset(true);
}
}
}
 
// renormalise one vector component of the DCM matrix
// this will return false if renormalization fails
bool
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
float renorm_val;
 
// numerical errors will slowly build up over time in DCM,
// causing inaccuracies. We can keep ahead of those errors
// using the renormalization technique from the DCM IMU paper
// (see equations 18 to 21).
 
// For APM we don't bother with the taylor expansion
// optimisation from the paper as on our 2560 CPU the cost of
// the sqrt() is 44 microseconds, and the small time saving of
// the taylor expansion is not worth the potential of
// additional error buildup.
 
// Note that we can get significant renormalisation values
// when we have a larger delta_t due to a glitch eleswhere in
// APM, such as a I2c timeout or a set of EEPROM writes. While
// we would like to avoid these if possible, if it does happen
// we don't want to compound the error by making DCM less
// accurate.
 
renorm_val = 1.0 / a.length();
 
// keep the average for reporting
_renorm_val_sum += renorm_val;
_renorm_val_count++;
 
if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
// this is larger than it should get - log it as a warning
renorm_range_count++;
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
// we are getting values which are way out of
// range, we will reset the matrix and hope we
// can recover our attitude using drift
// correction before we hit the ground!
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
// renorm_val);
printf("ERROR: DCM renormalisation error. renorm_val=%f\n", (double)renorm_val);
renorm_blowup_count++;
return false;
}
}
 
result = a * renorm_val;
return true;
}
 
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard
 
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions �renormalization�.
*/
void
AP_AHRS_DCM::normalize(void)
{
float error;
Vector3f t0, t1, t2;
 
//setCurrentProfiledActivity(MATRIX_NORMALIZE1);
 
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
t2 = t0 % t1;
 
//setCurrentProfiledActivity(MATRIX_NORMALIZE2);
if (!renorm(t0, _dcm_matrix.a) ||
!renorm(t1, _dcm_matrix.b) ||
!renorm(t2, _dcm_matrix.c)) {
// Our solution is blowing up and we will force back
// to last euler angles
reset(true);
}
}
 
 
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
Vector3f error;
float error_norm = 0;
float yaw_deltat = 0;
 
accel = _accel_vector;
 
// if enabled, use the GPS to correct our accelerometer vector
// for centripetal forces
if(_centripetal &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
accel_adjust(accel);
}
 
 
//*****Roll and Pitch***************
 
// normalise the accelerometer vector to a standard length
// this is important to reduce the impact of noise on the
// drift correction, as very noisy vectors tend to have
// abnormally high lengths. By normalising the length we
// reduce their impact.
float accel_length = accel.length();
 
accel *= (_gravity / accel_length);
if (accel.is_inf()) {
// we can't do anything useful with this sample
_omega_P.zero();
return;
}
 
// calculate the error, in m/s^2, between the attitude
// implied by the accelerometers and the attitude
// in the current DCM matrix
error = _dcm_matrix.c % accel;
 
// Limit max error to limit the effect of noisy values
// on the algorithm. This limits the error to about 11
// degrees
error_norm = error.length();
 
if (error_norm > 2) {
error *= (2 / error_norm);
}
 
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _kp_roll_pitch;
 
// the _omega_I is the long term accumulated gyro
// error. This determines how much gyro drift we can
// handle.
_omega_I_sum += error * (_ki_roll_pitch * deltat);
_omega_I_sum_time += deltat;
 
// these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm;
_error_rp_count++;
 
// yaw drift correction
 
// we only do yaw drift correction when we get a new yaw
// reference vector. In between times we rely on the gyros for
// yaw. Avoiding this calculation on every call to
// update_DCM() saves a lot of time
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
// Equation 23, Calculating YAW error
// We make the gyro YAW drift correction based
// on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
_compass_last_update = _compass->last_update;
} else {
// this is our first estimate of the yaw,
// or the compass has come back online after
// no readings for 2 seconds.
//
// construct a DCM matrix based on the current
// roll/pitch and the compass heading.
// First ensure the compass heading has been
// calculated
_compass->calculate(_dcm_matrix);
 
// now construct a new DCM matrix
_compass->null_offsets_disable();
_dcm_matrix.from_euler(roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
error_course = 0;
}
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
_gps_last_update = _gps->last_fix_time;
} else {
// when we first start moving, set the
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
if (_compass) {
_compass->null_offsets_disable();
}
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
if (_compass) {
_compass->null_offsets_enable();
}
_have_initial_yaw = true;
error_course = 0;
_gps_last_update = _gps->last_fix_time;
}
} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
// we are not going fast enough to use GPS for
// course correction, but we won't reset
// _have_initial_yaw yet, instead we just let
// the gyro handle yaw
error_course = 0;
} else {
// we are moving very slowly. Reset
// _have_initial_yaw and adjust our heading
// rapidly next time we get a good GPS ground
// speed
error_course = 0;
_have_initial_yaw = false;
}
}
}
 
// see if there is any error in our heading relative to the
// yaw reference. This will be zero most of the time, as we
// only calculate it when we get new data from the yaw
// reference source
if (yaw_deltat == 0 || error_course == 0) {
// we don't have a new reference heading. Slowly
// decay the _omega_yaw_P to ensure that if we have
// lost the yaw reference sensor completely we don't
// keep using a stale offset
_omega_yaw_P *= 0.97;
goto check_sum_time;
}
 
// ensure the course error is scaled from -PI to PI
if (error_course > PI) {
error_course -= 2*PI;
} else if (error_course < -PI) {
error_course += 2*PI;
}
 
// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
// this gives us an error in radians
error = _dcm_matrix.c * error_course;
 
// Adding yaw correction to proportional correction vector. We
// allow the yaw reference source to affect all 3 components
// of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero
_omega_yaw_P = error * _kp_yaw;
 
// add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x
// and y gyro drift correction. Using the compass or GPS for
// x/y drift correction is too inaccurate, and can lead to
// incorrect builups in the x/y drift. We rely on the
// accelerometers to get the x/y components right
_omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
 
// we keep the sum of yaw error for reporting via MAVLink.
_error_yaw_sum += error_course;
_error_yaw_count++;
 
check_sum_time:
if (_omega_I_sum_time > 10) {
// every 10 seconds we apply the accumulated
// _omega_I_sum changes to _omega_I. We do this to
// prevent short term feedback between the P and I
// terms of the controller. The _omega_I vector is
// supposed to refect long term gyro drift, but with
// high noise it can start to build up due to short
// term interactions. By summing it over 10 seconds we
// prevent the short term interactions and can apply
// the slope limit more accurately
float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
 
_omega_I += _omega_I_sum;
 
// zero the sum
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
}
 
 
// calculate the euler angles which will be used for high level
// navigation control
void
AP_AHRS_DCM::euler_angles(void)
{
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
}
 
/* reporting of DCM state for MAVLink */
 
// average error_roll_pitch since last call
float AP_AHRS_DCM::get_error_rp(void)
{
if (_error_rp_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_rp_last;
}
_error_rp_last = _error_rp_sum / _error_rp_count;
_error_rp_sum = 0;
_error_rp_count = 0;
return _error_rp_last;
}
 
// average error_yaw since last call
float AP_AHRS_DCM::get_error_yaw(void)
{
if (_error_yaw_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_yaw_last;
}
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
_error_yaw_sum = 0;
_error_yaw_count = 0;
return _error_yaw_last;
}
/branches/dongfang_FC_rewrite/AP_AHRS_DCM.h
0,0 → 1,92
#ifndef AP_AHRS_DCM_H
#define AP_AHRS_DCM_H
/*
DCM based AHRS (Attitude Heading Reference System) interface for
ArduPilot
 
This library 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 2.1 of the License, or (at your option) any later version.
*/
#include "AP_AHRS.h"
#include "GPS.h"
#include "Constants.h"
 
class AP_AHRS_DCM : public AP_AHRS {
public:
// Constructors
AP_AHRS_DCM(GPS *&gps) : AP_AHRS(gps) {
_kp_roll_pitch = 0.13;
_kp_yaw = 0.2;
_dcm_matrix(Vector3f(1, 0, 0),
Vector3f(0, 1, 0),
Vector3f(0, 0, 1));
 
// base the ki values on the sensors drift rate
_ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8;
_compass = NULL;
}
 
// return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) {return _omega; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
 
// return the current drift correction integrator value
Vector3f get_gyro_drift(void) {return _omega_I; }
 
// Methods
void update(int16_t attitude[3], float delta_t);
void reset(bool recover_eulers = false);
 
// status reporting
float get_error_rp(void);
float get_error_yaw(void);
 
float _kp_yaw;
 
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _ki_yaw;
bool _have_initial_yaw;
 
// Methods
void accel_adjust(Vector3f &accel);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat);
void euler_angles(void);
 
// primary representation of attitude
Matrix3f _dcm_matrix;
 
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _accel_vector; // current accel vector
 
Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_I_sum; // summation vector for omegaI
float _omega_I_sum_time;
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
 
// state to support status reporting
float _renorm_val_sum;
uint16_t _renorm_val_count;
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_rp_last;
float _error_yaw_sum;
uint16_t _error_yaw_count;
float _error_yaw_last;
 
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
};
 
#endif // AP_AHRS_DCM_H
/branches/dongfang_FC_rewrite/AP_Compass.h
0,0 → 1,7
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
/// @file AP_Compass.h
/// @brief Catch-all header that defines all supported compass classes.
 
//#include "AP_Compass_HMC5843.h"
#include "AP_Compass_HIL.h"
/branches/dongfang_FC_rewrite/AP_Compass_HIL.cpp
0,0 → 1,30
/*
AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
Code by James Goppert. DIYDrones.com
 
This library 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 2.1 of the License, or (at your option) any later version.
 
#include "AP_Compass_HIL.h"
 
// Public Methods //////////////////////////////////////////////////////////////
 
bool AP_Compass_HIL::read()
{
// values set by setHIL function
last_update = 0; //micros(); // record time of update
return true;
}
 
// Update raw magnetometer values from HIL data
//
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) {
Vector3f ofs = _offset;// .get();
mag_x = _mag_x + ofs.x;
mag_y = _mag_y + ofs.y;
mag_z = _mag_z + ofs.z;
healthy = true;
}
*/
/branches/dongfang_FC_rewrite/AP_Compass_HIL.h
0,0 → 1,15
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_Compass_HIL_H
#define AP_Compass_HIL_H
 
#include "Compass.h"
 
class AP_Compass_HIL : public Compass
{
public:
AP_Compass_HIL() : Compass() { product_id = AP_COMPASS_TYPE_HIL; }
bool read(void);
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
};
 
#endif
/branches/dongfang_FC_rewrite/AP_GPS.h
0,0 → 1,9
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
/// @file AP_GPS.h
/// @brief Catch-all header that defines all supported GPS classes.
 
#include "AP_GPS_None.h"
//#include "AP_GPS_Auto.h"
//#include "AP_GPS_Shim.h" // obsoletes AP_GPS_HIL, use in preference
//#include "AP_GPS_IMU.h" // temporarily reinstated for Xplane support
/branches/dongfang_FC_rewrite/AP_GPS_None.h
0,0 → 1,18
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
#ifndef AP_GPS_None_h
#define AP_GPS_None_h
 
#include "GPS.h"
 
class AP_GPS_None : public GPS
{
public:
AP_GPS_None(void *s) : GPS(s) {}
virtual void init(void) {}
 
virtual bool read(void) {
return false;
};
};
#endif
/branches/dongfang_FC_rewrite/AP_Math.cpp
0,0 → 1,70
#include "AP_Math.h"
#include <math.h>
 
// a varient of asin() that checks the input ranges and ensures a
// valid angle as output. If nan is given as input then zero is
// returned.
float safe_asin(float v)
{
if (isnan(v)) {
return 0.0;
}
if (v >= 1.0) {
return M_PI/2;
}
if (v <= -1.0) {
return -M_PI/2;
}
return asin(v);
}
 
// a varient of sqrt() that checks the input ranges and ensures a
// valid value as output. If a negative number is given then 0 is
// returned. The reasoning is that a negative number for sqrt() in our
// code is usually caused by small numerical rounding errors, so the
// real input should have been zero
float safe_sqrt(float v)
{
float ret = sqrt(v);
if (isnan(ret)) {
return 0;
}
return ret;
}
 
 
// find a rotation that is the combination of two other
// rotations. This is used to allow us to add an overall board
// rotation to an existing rotation of a sensor such as the compass
// Note that this relies the set of rotations being complete. The
// optional 'found' parameter is for the test suite to ensure that it is.
enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)
{
Vector3f tv1, tv2;
enum Rotation r;
tv1(1,2,3);
tv1.rotate(r1);
tv1.rotate(r2);
 
for (r=ROTATION_NONE; r<ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) {
Vector3f diff;
tv2(1,2,3);
tv2.rotate(r);
diff = tv1 - tv2;
if (diff.length() < 1.0e-6) {
// we found a match
if (found) {
*found = true;
}
return r;
}
}
 
// we found no matching rotation. Someone has edited the
// rotations list and broken its completeness property ...
if (found) {
*found = false;
}
return ROTATION_NONE;
}
/branches/dongfang_FC_rewrite/AP_Math.h
0,0 → 1,30
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
#ifndef AP_MATH_H
#define AP_MATH_H
// Assorted useful math operations for ArduPilot(Mega)
//#include <AP_Common.h>
#include <stdint.h>
//#include "rotations.h"
//#include "vector2.h"
#include "Vector3.h"
#include "Matrix3.h"
//#include "quaternion.h"
//#include "polygon.h"
 
// define AP_Param types AP_Vector3f and Ap_Matrix3f
// AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
// AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 
// a varient of asin() that always gives a valid answer.
float safe_asin(float v);
 
// a varient of sqrt() that always gives a valid answer.
float safe_sqrt(float v);
 
// find a rotation that is the combination of two other
// rotations. This is used to allow us to add an overall board
// rotation to an existing rotation of a sensor such as the compass
// enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
 
#endif
/branches/dongfang_FC_rewrite/Compass.cpp
0,0 → 1,301
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h"
 
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
Compass::Compass(void) :
product_id(AP_COMPASS_TYPE_UNKNOWN),
_declination (0.0),
_learn(1),
_use_for_yaw(1),
_null_enable(false),
_null_init_done(false),
_auto_declination(1),
_orientation(ROTATION_NONE)
{
}
 
// Default init method, just returns success.
//
bool
Compass::init()
{
return true;
}
 
/* set_orientation
void
Compass::set_orientation(enum Rotation rotation)
{
_orientation = rotation;
}
*/
 
/* set_offsets
void
Compass::set_offsets(const Vector3f &offsets)
{
_offset.set(offsets);
}
*/
 
/* save_offsets
void
Compass::save_offsets()
{
_offset.save();
}
*/
 
/* get_offsets
Vector3f &
Compass::get_offsets()
{
return _offset;
}
*/
 
/* set_initial_location
void
Compass::set_initial_location(long latitude, long longitude)
{
// if automatic declination is configured, then compute
// the declination based on the initial GPS fix
if (_auto_declination) {
// Set the declination based on the lat/lng from GPS
null_offsets_disable();
_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
null_offsets_enable();
}
}
*/
 
/* set_declination
void
Compass::set_declination(float radians)
{
_declination.set_and_save(radians);
}
*/
 
/* get_declination
float
Compass::get_declination()
{
return _declination.get();
}
*/
 
/* calculate
void
Compass::calculate(float roll, float pitch)
{
// Note - This function implementation is deprecated
// The alternate implementation of this function using the dcm matrix is preferred
float headX;
float headY;
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);
 
// Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch;
// Tilt compensated magnetic field Y component:
headY = mag_y*cos_roll - mag_z*sin_roll;
// magnetic heading
heading = atan2(-headY,headX);
 
// Declination correction (if supplied)
if( fabs(_declination) > 0.0 )
{
heading = heading + _declination;
if (heading > M_PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0 * M_PI);
else if (heading < -M_PI)
heading += (2.0 * M_PI);
}
 
// Optimization for external DCM use. Calculate normalized components
heading_x = cos(heading);
heading_y = sin(heading);
}
*/
 
void
Compass::calculate(const Matrix3f &dcm_matrix)
{
float headX;
float headY;
float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
// sin(pitch) = - dcm_matrix(3,1)
// cos(pitch)*sin(roll) = - dcm_matrix(3,2)
// cos(pitch)*cos(roll) = - dcm_matrix(3,3)
 
if (cos_pitch == 0.0) {
// we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again.
return;
}
 
// Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
// Tilt compensated magnetic field Y component:
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain(atan2(-headY,headX), -3.15, 3.15);
 
// Declination correction (if supplied)
if( fabs(_declination) > 0.0 )
{
heading = heading + _declination;
if (heading > M_PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0 * M_PI);
else if (heading < -M_PI)
heading += (2.0 * M_PI);
}
 
// Optimization for external DCM use. Calculate normalized components
heading_x = cos(heading);
heading_y = sin(heading);
 
#if 0
if (isnan(heading_x) || isnan(heading_y)) {
Serial.printf("COMPASS: c.x %f c.y %f c.z %f cos_pitch %f mag_x %d mag_y %d mag_z %d headX %f headY %f heading %f heading_x %f heading_y %f\n",
dcm_matrix.c.x,
dcm_matrix.c.y,
dcm_matrix.c.x,
cos_pitch,
(int)mag_x, (int)mag_y, (int)mag_z,
headX, headY,
heading,
heading_x, heading_y);
}
#endif
}
 
 
/*
this offset nulling algorithm is inspired by this paper from Bill Premerlani
 
http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
 
The base algorithm works well, but is quite sensitive to
noise. After long discussions with Bill, the following changes were
made:
 
1) we keep a history buffer that effectively divides the mag
vectors into a set of N streams. The algorithm is run on the
streams separately
 
2) within each stream we only calculate a change when the mag
vector has changed by a significant amount.
 
This gives us the property that we learn quickly if there is no
noise, but still learn correctly (and slowly) in the face of lots of
noise.
*/
/* null_offsets
void
Compass::null_offsets(void)
{
if (_null_enable == false || _learn == 0) {
// auto-calibration is disabled
return;
}
 
// this gain is set so we converge on the offsets in about 5
// minutes with a 10Hz compass
const float gain = 0.01;
const float max_change = 10.0;
const float min_diff = 50.0;
Vector3f ofs;
 
ofs = _offset.get();
 
if (!_null_init_done) {
// first time through
_null_init_done = true;
for (uint8_t i=0; i<_mag_history_size; i++) {
// fill the history buffer with the current mag vector,
// with the offset removed
_mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
}
_mag_history_index = 0;
return;
}
 
Vector3f b1, b2, diff;
float length;
 
// get a past element
b1 = Vector3f(_mag_history[_mag_history_index].x,
_mag_history[_mag_history_index].y,
_mag_history[_mag_history_index].z);
// the history buffer doesn't have the offsets
b1 += ofs;
 
// get the current vector
b2 = Vector3f(mag_x, mag_y, mag_z);
 
// calculate the delta for this sample
diff = b2 - b1;
length = diff.length();
if (length < min_diff) {
// the mag vector hasn't changed enough - we don't get
// enough information from this vector to use it.
// Note that we don't put the current vector into the mag
// history here. We want to wait for a larger rotation to
// build up before calculating an offset change, as accuracy
// of the offset change is highly dependent on the size of the
// rotation.
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
return;
}
 
// put the vector in the history
_mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
 
// equation 6 of Bills paper
diff = diff * (gain * (b2.length() - b1.length()) / length);
 
// limit the change from any one reading. This is to prevent
// single crazy readings from throwing off the offsets for a long
// time
length = diff.length();
if (length > max_change) {
diff *= max_change / length;
}
 
// set the new offsets
_offset.set(_offset.get() - diff);
}
*/
 
// Have no idea why this is necessary:
bool Compass::read(void) {
return false;
}
 
void
Compass::null_offsets_enable(void)
{
_null_init_done = false;
_null_enable = true;
}
 
void
Compass::null_offsets_disable(void)
{
_null_init_done = false;
_null_enable = false;
}
/branches/dongfang_FC_rewrite/Compass.h
0,0 → 1,137
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef Compass_h
#define Compass_h
 
#include <inttypes.h>
#include "AP_Math.h"
 
// compass product id
#define AP_COMPASS_TYPE_UNKNOWN 0x00
#define AP_COMPASS_TYPE_HIL 0x01
#define AP_COMPASS_TYPE_HMC5843 0x02
#define AP_COMPASS_TYPE_HMC5883L 0x03
 
class Compass
{
public:
int16_t product_id; /// product id
int16_t mag_x; ///< magnetic field strength along the X axis
int16_t mag_y; ///< magnetic field strength along the Y axis
int16_t mag_z; ///< magnetic field strength along the Z axis
float heading; ///< compass heading in radians
float heading_x; ///< compass vector X magnitude
float heading_y; ///< compass vector Y magnitude
uint32_t last_update; ///< micros() time of last update
bool healthy; ///< true if last read OK
 
/// Constructor
///
Compass();
 
/// Initialize the compass device.
///
/// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning.
///
virtual bool init();
/// Read the compass and update the mag_ variables.
///
virtual bool read(void) /* = 0 */;
 
/// Calculate the tilt-compensated heading_ variables.
///
/// @param roll The current airframe roll angle.
/// @param pitch The current airframe pitch angle.
///
// virtual void calculate(float roll, float pitch);
 
/// Calculate the tilt-compensated heading_ variables.
///
/// @param dcm_matrix The current orientation rotation matrix
///
virtual void calculate(const Matrix3f &dcm_matrix);
 
/// Set the compass orientation matrix, used to correct for
/// various compass mounting positions.
///
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
/// to the body frame.
///
//virtual void set_orientation(enum Rotation rotation);
 
/// Sets the compass offset x/y/z values.
///
/// @param offsets Offsets to the raw mag_ values.
///
// virtual void set_offsets(const Vector3f &offsets);
 
/// Saves the current compass offset x/y/z values.
///
/// This should be invoked periodically to save the offset values maintained by
/// ::null_offsets.
///
// virtual void save_offsets();
 
/// Returns the current offset values
///
/// @returns The current compass offsets.
///
// virtual Vector3f &get_offsets();
 
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
// void set_initial_location(long latitude, long longitude);
 
/// Program new offset values.
///
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
// void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
 
/// Perform automatic offset updates
///
// void null_offsets(void);
 
/// Enable/Start automatic offset updates
///
void null_offsets_enable(void);
 
 
/// Disable/Stop automatic offset updates
///
void null_offsets_disable(void);
 
/// return true if the compass should be used for yaw calculations
bool use_for_yaw(void) { return healthy && _use_for_yaw; }
 
/// Sets the local magnetic field declination.
///
/// @param radians Local field declination.
///
// virtual void set_declination(float radians);
float get_declination();
 
// static const struct AP_Param::GroupInfo var_info[];
 
protected:
float _declination;
int8_t _learn; ///<enable calibration learning
int8_t _use_for_yaw; ///<enable use for yaw calculation
bool _null_enable; ///< enabled flag for offset nulling
bool _null_init_done; ///< first-time-around flag used by offset nulling
int8_t _auto_declination; ///<enable automatic declination code
enum Rotation _orientation;
 
Vector3f _offset;
///< used by offset correction
static const uint8_t _mag_history_size = 20;
uint8_t _mag_history_index;
Vector3i _mag_history[_mag_history_size];
};
#endif
/branches/dongfang_FC_rewrite/Constants.h
0,0 → 1,58
#ifndef CONSTANTS_h
#define CONSTANTS_h
 
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define PI 3.1415926535897932384626433832795
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
#define ToRad(x) ((x)*DEG_TO_RAD) // *pi/180
#define degrees(rad) ((rad)*RAD_TO_DEG)
#define INT16DEG_PI_FACTOR ((1L<<15) / PI)
 
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* rotations.h
* Copyright (C) Andrew Tridgell 2012
*
* This file 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, either version 3 of the License, or
* (at your option) any later version.
*
* This file 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/>.
*/
 
// these rotations form a full set - every rotation in the following
// list when combined with another in the list forms an entry which is
// also in the list. This is an important property. Please run the
// rotations test suite if you add to the list.
 
// these rotation values are stored to EEPROM, so be careful not to
// change the numbering of any existing entry when adding a new entry.
enum Rotation {
ROTATION_NONE = 0,
ROTATION_YAW_45,
ROTATION_YAW_90,
ROTATION_YAW_135,
ROTATION_YAW_180,
ROTATION_YAW_225,
ROTATION_YAW_270,
ROTATION_YAW_315,
ROTATION_ROLL_180,
ROTATION_ROLL_180_YAW_45,
ROTATION_ROLL_180_YAW_90,
ROTATION_ROLL_180_YAW_135,
ROTATION_PITCH_180,
ROTATION_ROLL_180_YAW_225,
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_315,
ROTATION_MAX
};
 
#endif
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
1,11 → 1,13
#include "sensors.h"
#include "printf_P.h"
#include "analog.h"
#include "twimaster.h"
#include "configuration.h"
#include "eeprom.h"
#include "timer0.h"
#include "output.h"
 
#include <stdio.h>
 
void I2C_OutputAmplifierOffsets(void) {
uint16_t timeout = setDelay(2000);
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
19,39 → 21,50
}
}
 
uint8_t DACChannelFor(uint8_t axis) {
switch (axis) {
case X:
return 1;
case Y:
return 0;
case Z:
return 2;
default:
return -1; // should never happen.
}
}
 
void gyro_calibrate(void) {
printf("gyro_calibrate");
uint8_t i, axis, factor, numberOfAxesInRange = 0;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
for (i = 140; i != 0; i--) {
delay_ms_with_adc_measurement(i <= 10 ? 10 : 2, 1);
 
for (i = 200; i != 0; i--) {
waitADCCycle(i <= 10 ? 10 : 2);
// If all 3 axis are in range, shorten the remaining number of iterations.
if (numberOfAxesInRange == 3 && i > 10) i = 10;
numberOfAxesInRange = 0;
 
for (axis = PITCH; axis <= YAW; axis++) {
if (axis == YAW)
factor = GYRO_OVERSAMPLING_YAW;
for (axis=X; axis<=Z; axis++) {
uint8_t dacChannel = DACChannelFor(axis);
if (axis==Z)
factor=GYRO_OVERSAMPLING_Z;
else
factor = GYRO_OVERSAMPLING_PITCHROLL;
factor=GYRO_OVERSAMPLING_XY;
 
if (rawGyroValue(axis) < 510 * factor)
gyroAmplifierOffset.offsets[axis]--;
else if (rawGyroValue(axis) > 515 * factor)
gyroAmplifierOffset.offsets[axis]++;
if (gyroValueForFC13DACCalibration(axis) < (uint16_t)(510*factor))
gyroAmplifierOffset.offsets[dacChannel]--;
else if (gyroValueForFC13DACCalibration(axis) > (uint16_t)(515 * factor))
gyroAmplifierOffset.offsets[dacChannel]++;
else
numberOfAxesInRange++;
 
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */
if (gyroAmplifierOffset.offsets[axis] < 10) {
gyroAmplifierOffset.offsets[axis] = 10;
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis);
} else if (gyroAmplifierOffset.offsets[axis] > 245) {
gyroAmplifierOffset.offsets[axis] = 245;
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis);
if (gyroAmplifierOffset.offsets[dacChannel] < 10) {
gyroAmplifierOffset.offsets[dacChannel] = 10;
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis);
} else if (gyroAmplifierOffset.offsets[dacChannel] > 245) {
gyroAmplifierOffset.offsets[dacChannel] = 245;
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis);
}
}
58,15 → 71,15
I2C_OutputAmplifierOffsets();
}
gyroAmplifierOffset_writeToEEProm();
delay_ms_with_adc_measurement(70, 0);
waitADCCycle(70);
}
 
void gyro_init(void) {
if (gyroAmplifierOffset_readFromEEProm()) {
printf("gyro amp invalid, recalibrate.");
gyroAmplifierOffset.offsets[PITCH] =
gyroAmplifierOffset.offsets[ROLL] =
gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0);
gyroAmplifierOffset.offsets[X] =
gyroAmplifierOffset.offsets[Y] =
gyroAmplifierOffset.offsets[Z] = (uint8_t)(255 * 1.2089 / 3.0);
} else {
I2C_OutputAmplifierOffsets();
}
73,11 → 86,8
}
 
void gyro_setDefaultParameters(void) {
IMUConfig.gyroQuadrant = 0;
IMUConfig.gyroQuadrant = 4;
IMUConfig.accQuadrant = 4;
IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_YAW | IMU_REVERSE_ACC_XY;
IMUConfig.imuReversedFlags = IMU_REVERSE_ACCEL_Z | IMU_REVERSE_GYRO_Z;
staticParams.gyroD = 3;
IMUConfig.driftCompDivider = 1;
IMUConfig.driftCompLimit = 200;
IMUConfig.zerothOrderCorrection = 120;
}
/branches/dongfang_FC_rewrite/GPS.cpp
0,0 → 1,70
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
#include "GPS.h"
/*
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
*/
 
void
GPS::update(void)
{
bool result;
 
// call the GPS driver to process incoming data
result = read();
 
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((millis() - _idleTimer) > idleTimeout) {
_status = NO_GPS;
 
init();
// reset the idle timer
_idleTimer = millis();
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
 
valid_read = true;
new_data = true;
 
// reset the idle timer
_idleTimer = millis();
 
if (_status == GPS_OK) {
// update our acceleration
float deltat = 1.0e-3 * (_idleTimer - last_fix_time);
float deltav = 1.0e-2 * ((float)ground_speed - (float)_last_ground_speed);
last_fix_time = _idleTimer;
_last_ground_speed = ground_speed;
 
if (deltat > 2.0 || deltat == 0) {
// we didn't get a fix for 2 seconds - set
// acceleration to zero, as the estimate will be too
// far out
_acceleration = 0;
} else {
// calculate a mildly smoothed acceleration value
_acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat));
}
}
}
}
 
void
GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{
}
 
// XXX this is probably the wrong way to do it, too
void
GPS::_error(const char *msg)
{
Serial.println(msg);
}
/branches/dongfang_FC_rewrite/GPS.h
0,0 → 1,217
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
/// @file GPS.h
/// @brief Interface definition for the various GPS drivers.
 
#ifndef GPS_h
#define GPS_h
 
#include <inttypes.h>
//#include "Stream.h"
 
/// @class GPS
/// @brief Abstract base class for GPS receiver drivers.
class GPS
{
public:
 
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
 
void (*callback)(unsigned long t);
 
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked
};
 
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) {
return _status;
}
 
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
 
 
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) {
return _epoch;
}
 
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(void) = 0;
 
// Properties
long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
 
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
 
// Deprecated properties
bool fix; ///< true if we have a position fix (use ::status instead)
bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
 
// Debug support
bool print_errors; ///< deprecated
 
// HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
 
/// Time in milliseconds after which we will assume the GPS is no longer
/// sending us updates and attempt a re-init.
///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate.
///
uint32_t idleTimeout;
 
// our approximate linear acceleration in m/s/s
float acceleration(void) { return _acceleration; }
 
// the time we got our last fix in system milliseconds
uint32_t last_fix_time;
 
protected:
/// Stream *_port; ///< port the GPS is attached to
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param s Stream connected to the GPS module.
///
GPS(void *s) {};
 
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
 
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
 
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes);
 
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
 
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
 
private:
 
 
/// Last time that the GPS driver got a good packet from the GPS
///
uint32_t _idleTimer;
 
/// Our current status
GPS_Status _status;
 
// previous ground speed in cm/s
uint32_t _last_ground_speed;
 
// smoothed estimate of our acceleration
float _acceleration;
};
 
inline long
GPS::_swapl(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
 
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
 
return(u.v);
}
 
inline int16_t
GPS::_swapi(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
 
u.b[0] = b[1];
u.b[1] = b[0];
 
return(u.v);
}
 
#endif
/branches/dongfang_FC_rewrite/Matrix3.cpp
0,0 → 1,199
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* matrix3.cpp
* Copyright (C) Andrew Tridgell 2012
*
* This file 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, either version 3 of the License, or
* (at your option) any later version.
*
* This file 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 "AP_Math.h"
 
#define HALF_SQRT_2 0.70710678118654757
 
#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define MATRIX_ROTATION_YAW_45 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_135 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_225 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_315 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)
#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1)
#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1)
 
// fill in a matrix with a standard rotation
template <typename T>
void Matrix3<T>::rotation(enum Rotation r)
{
switch (r) {
case ROTATION_NONE:
case ROTATION_MAX:
*this = MATRIX_ROTATION_NONE;
break;
case ROTATION_YAW_45:
*this = MATRIX_ROTATION_YAW_45;
break;
case ROTATION_YAW_90:
*this = MATRIX_ROTATION_YAW_90;
break;
case ROTATION_YAW_135:
*this = MATRIX_ROTATION_YAW_135;
break;
case ROTATION_YAW_180:
*this = MATRIX_ROTATION_YAW_180;
break;
case ROTATION_YAW_225:
*this = MATRIX_ROTATION_YAW_225;
break;
case ROTATION_YAW_270:
*this = MATRIX_ROTATION_YAW_270;
break;
case ROTATION_YAW_315:
*this = MATRIX_ROTATION_YAW_315;
break;
case ROTATION_ROLL_180:
*this = MATRIX_ROTATION_ROLL_180;
break;
case ROTATION_ROLL_180_YAW_45:
*this = MATRIX_ROTATION_ROLL_180_YAW_45;
break;
case ROTATION_ROLL_180_YAW_90:
*this = MATRIX_ROTATION_ROLL_180_YAW_90;
break;
case ROTATION_ROLL_180_YAW_135:
*this = MATRIX_ROTATION_ROLL_180_YAW_135;
break;
case ROTATION_PITCH_180:
*this = MATRIX_ROTATION_PITCH_180;
break;
case ROTATION_ROLL_180_YAW_225:
*this = MATRIX_ROTATION_ROLL_180_YAW_225;
break;
case ROTATION_ROLL_180_YAW_270:
*this = MATRIX_ROTATION_ROLL_180_YAW_270;
break;
case ROTATION_ROLL_180_YAW_315:
*this = MATRIX_ROTATION_ROLL_180_YAW_315;
break;
}
}
 
// create a rotation matrix given some euler angles
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
template <typename T>
void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
{
float cp = cos(pitch);
float sp = sin(pitch);
float sr = sin(roll);
float cr = cos(roll);
float sy = sin(yaw);
float cy = cos(yaw);
 
a.x = cp * cy;
a.y = (sr * sp * cy) - (cr * sy);
a.z = (cr * sp * cy) + (sr * sy);
b.x = cp * sy;
b.y = (sr * sp * sy) + (cr * cy);
b.z = (cr * sp * sy) - (sr * cy);
c.x = -sp;
c.y = sr * cp;
c.z = cr * cp;
}
 
// calculate euler angles from a rotation matrix
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
template <typename T>
void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
{
if (pitch != NULL) {
*pitch = -safe_asin(c.x);
}
if (roll != NULL) {
*roll = atan2(c.y, c.z);
}
if (yaw != NULL) {
*yaw = atan2(b.x, a.x);
}
}
 
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
template <typename T>
void Matrix3<T>::rotate(const Vector3<T> &g)
{
Matrix3f temp_matrix;
temp_matrix.a.x = a.y * g.z - a.z * g.y;
temp_matrix.a.y = a.z * g.x - a.x * g.z;
temp_matrix.a.z = a.x * g.y - a.y * g.x;
temp_matrix.b.x = b.y * g.z - b.z * g.y;
temp_matrix.b.y = b.z * g.x - b.x * g.z;
temp_matrix.b.z = b.x * g.y - b.y * g.x;
temp_matrix.c.x = c.y * g.z - c.z * g.y;
temp_matrix.c.y = c.z * g.x - c.x * g.z;
temp_matrix.c.z = c.x * g.y - c.y * g.x;
 
(*this) += temp_matrix;
}
 
 
// multiplication by a vector
template <typename T>
Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
{
return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,
b.x * v.x + b.y * v.y + b.z * v.z,
c.x * v.x + c.y * v.y + c.z * v.z);
}
 
// multiplication of transpose by a vector
template <typename T>
Vector3<T> Matrix3<T>::mul_transpose(const Vector3<T> &v) const
{
return Vector3<T>(a.x * v.x + b.x * v.y + c.x * v.z,
a.y * v.x + b.y * v.y + c.y * v.z,
a.z * v.x + b.z * v.y + c.z * v.z);
}
 
// multiplication by another Matrix3<T>
template <typename T>
Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const
{
Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
return temp;
}
 
 
// only define for float
template void Matrix3<float>::rotation(enum Rotation);
template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;
/branches/dongfang_FC_rewrite/Matrix3.h
0,0 → 1,154
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
// Copyright 2010 Michael Smith, all rights reserved.
 
// This library 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 2.1 of the License, or (at your option) any later version.
 
// Inspired by:
/****************************************
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
*/
 
//
// 3x3 matrix implementation.
//
// Note that the matrix is organised in row-normal form (the same as
// applies to array indexing).
//
// In addition to the template, this header defines the following types:
//
// Matrix3i 3x3 matrix of signed integers
// Matrix3ui 3x3 matrix of unsigned integers
// Matrix3l 3x3 matrix of signed longs
// Matrix3ul 3x3 matrix of unsigned longs
// Matrix3f 3x3 matrix of signed floats
//
 
#ifndef MATRIX3_H
#define MATRIX3_H
 
#include "Vector3.h"
#include "Constants.h"
 
// 3x3 matrix with elements of type T
template <typename T>
class Matrix3 {
public:
 
// Vectors comprising the rows of the matrix
Vector3<T> a, b, c;
 
// trivial ctor
// note that the Vector3 ctor will zero the vector elements
Matrix3<T>() {}
 
// setting ctor
Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
 
// setting ctor
Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {}
 
// function call operator
void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
{ a = a0; b = b0; c = c0; }
 
// test for equality
bool operator == (const Matrix3<T> &m)
{ return (a==m.a && b==m.b && c==m.c); }
 
// test for inequality
bool operator != (const Matrix3<T> &m)
{ return (a!=m.a || b!=m.b || c!=m.c); }
 
// negation
Matrix3<T> operator - (void) const
{ return Matrix3<T>(-a,-b,-c); }
 
// addition
Matrix3<T> operator + (const Matrix3<T> &m) const
{ return Matrix3<T>(a+m.a, b+m.b, c+m.c); }
Matrix3<T> &operator += (const Matrix3<T> &m)
{ return *this = *this + m; }
 
// subtraction
Matrix3<T> operator - (const Matrix3<T> &m) const
{ return Matrix3<T>(a-m.a, b-m.b, c-m.c); }
Matrix3<T> &operator -= (const Matrix3<T> &m)
{ return *this = *this - m; }
 
// uniform scaling
Matrix3<T> operator * (const T num) const
{ return Matrix3<T>(a*num, b*num, c*num); }
Matrix3<T> &operator *= (const T num)
{ return *this = *this * num; }
Matrix3<T> operator / (const T num) const
{ return Matrix3<T>(a/num, b/num, c/num); }
Matrix3<T> &operator /= (const T num)
{ return *this = *this / num; }
 
// multiplication by a vector
Vector3<T> operator *(const Vector3<T> &v) const;
 
// multiplication of transpose by a vector
Vector3<T> mul_transpose(const Vector3<T> &v) const;
 
// multiplication by another Matrix3<T>
Matrix3<T> operator *(const Matrix3<T> &m) const;
 
Matrix3<T> &operator *=(const Matrix3<T> &m)
{ return *this = *this * m; }
 
// transpose the matrix
Matrix3<T> transposed(void) const
{
return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),
Vector3<T>(a.y, b.y, c.y),
Vector3<T>(a.z, b.z, c.z));
}
Matrix3<T> transpose(void)
{ return *this = transposed(); }
 
// zero the matrix
void zero(void) {
a.x = a.y = a.z = 0;
b.x = b.y = b.z = 0;
c.x = c.y = c.z = 0;
}
 
// setup the identity matrix
void identity(void) {
a.x = b.y = c.z = 1;
a.y = a.z = 0;
b.x = b.z = 0;
c.x = c.y = 0;
}
 
// check if any elements are NAN
bool is_nan(void)
{ return a.is_nan() || b.is_nan() || c.is_nan(); }
 
// fill in the matrix with a standard rotation
void rotation(enum Rotation rotation);
 
// create a rotation matrix from Euler angles
void from_euler(float roll, float pitch, float yaw);
 
// create eulers from a rotation matrix
void to_euler(float *roll, float *pitch, float *yaw);
 
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
void rotate(const Vector3<T> &g);
};
 
typedef Matrix3<int16_t> Matrix3i;
typedef Matrix3<uint16_t> Matrix3ui;
typedef Matrix3<int32_t> Matrix3l;
typedef Matrix3<uint32_t> Matrix3ul;
typedef Matrix3<float> Matrix3f;
 
#endif // MATRIX3_H
/branches/dongfang_FC_rewrite/Vector3.cpp
0,0 → 1,115
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* vector3.cpp
* Copyright (C) Andrew Tridgell 2012
*
* This file 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, either version 3 of the License, or
* (at your option) any later version.
*
* This file 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 "AP_Math.h"
 
#define HALF_SQRT_2 0.70710678118654757
 
// rotate a vector by a standard rotation, attempting
// to use the minimum number of floating point operations
template <typename T>
void Vector3<T>::rotate(enum Rotation rotation)
{
T tmp;
switch (rotation) {
case ROTATION_NONE:
case ROTATION_MAX:
return;
case ROTATION_YAW_45: {
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_YAW_90: {
tmp = x; x = -y; y = tmp;
return;
}
case ROTATION_YAW_135: {
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return;
}
case ROTATION_YAW_180:
x = -x; y = -y;
return;
case ROTATION_YAW_225: {
tmp = HALF_SQRT_2*(y - x);
y = -HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_YAW_270: {
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_YAW_315: {
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp;
return;
}
case ROTATION_ROLL_180: {
y = -y; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_45: {
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_90: {
tmp = x; x = y; y = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_135: {
tmp = HALF_SQRT_2*(y - x);
y = HALF_SQRT_2*(y + x);
x = tmp; z = -z;
return;
}
case ROTATION_PITCH_180: {
x = -x; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_225: {
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_270: {
tmp = x; x = -y; y = -tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_315: {
tmp = HALF_SQRT_2*(x - y);
y = -HALF_SQRT_2*(x + y);
x = tmp; z = -z;
return;
}
}
}
 
// only define for signed numbers
template void Vector3<float>::rotate(enum Rotation);
template void Vector3<int16_t>::rotate(enum Rotation);
template void Vector3<int32_t>::rotate(enum Rotation);
/branches/dongfang_FC_rewrite/Vector3.h
0,0 → 1,211
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
// Copyright 2010 Michael Smith, all rights reserved.
 
// This library 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 2.1 of the License, or (at your option) any later version.
 
// Derived closely from:
/****************************************
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
* Original: 9-16-2002
* Revised: 19-11-2003
* 11-12-2003
* 18-12-2003
* 06-06-2004
*
* � 2003, This code is provided "as is" and you can use it freely as long as
* credit is given to Bill Perone in the application it is used in
*
* Notes:
* if a*b = 0 then a & b are orthogonal
* a%b = -b%a
* a*(b%c) = (a%b)*c
* a%b = a(cast to matrix)*b
* (a%b).length() = area of parallelogram formed by a & b
* (a%b).length() = a.length()*b.length() * sin(angle between a & b)
* (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
* a * (b%c) = volume of parallelpiped formed by a, b, c
* vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
* scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
* vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
* if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
* vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
* or if the matrix (A) * b = 0
*
****************************************/
 
#ifndef VECTOR3_H
#define VECTOR3_H
 
#include <math.h>
#include <string.h>
#include "Constants.h"
#include "Vector3.h"
 
template <typename T>
class Vector3
{
public:
T x, y, z;
 
// trivial ctor
Vector3<T>() { x = y = z = 0; }
 
// setting ctor
Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {}
 
// function call operator
void operator ()(const T x0, const T y0, const T z0)
{ x= x0; y= y0; z= z0; }
 
// indexing operator
T operator [](uint8_t i)
{ switch(i) {
case 0: return x;
case 1: return y;
case 2: return z;
default: return 0;
}
}
 
// test for equality
bool operator==(const Vector3<T> &v)
{ return (x==v.x && y==v.y && z==v.z); }
 
// test for inequality
bool operator!=(const Vector3<T> &v)
{ return (x!=v.x || y!=v.y || z!=v.z); }
 
// negation
Vector3<T> operator -(void) const
{ return Vector3<T>(-x,-y,-z); }
 
// addition
Vector3<T> operator +(const Vector3<T> &v) const
{ return Vector3<T>(x+v.x, y+v.y, z+v.z); }
 
// subtraction
Vector3<T> operator -(const Vector3<T> &v) const
{ return Vector3<T>(x-v.x, y-v.y, z-v.z); }
 
// uniform scaling
Vector3<T> operator *(const T num) const
{
Vector3<T> temp(*this);
return temp*=num;
}
 
// uniform scaling
Vector3<T> operator /(const T num) const
{
Vector3<T> temp(*this);
return temp/=num;
}
 
// addition
Vector3<T> &operator +=(const Vector3<T> &v)
{
x+=v.x; y+=v.y; z+=v.z;
return *this;
}
 
// subtraction
Vector3<T> &operator -=(const Vector3<T> &v)
{
x-=v.x; y-=v.y; z-=v.z;
return *this;
}
 
// uniform scaling
Vector3<T> &operator *=(const T num)
{
x*=num; y*=num; z*=num;
return *this;
}
 
// uniform scaling
Vector3<T> &operator /=(const T num)
{
x/=num; y/=num; z/=num;
return *this;
}
 
// dot product
T operator *(const Vector3<T> &v) const
{ return x*v.x + y*v.y + z*v.z; }
 
// cross product
Vector3<T> operator %(const Vector3<T> &v) const
{
Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
return temp;
}
 
// gets the length of this vector squared
T length_squared() const
{ return (T)(*this * *this); }
 
// gets the length of this vector
float length() const
{ return (T)sqrt(*this * *this); }
 
// normalizes this vector
void normalize()
{ *this/=length(); }
 
// zero the vector
void zero()
{ x = y = z = 0.0; }
 
// returns the normalized version of this vector
Vector3<T> normalized() const
{ return *this/length(); }
 
// reflects this vector about n
void reflect(const Vector3<T> &n)
{
Vector3<T> orig(*this);
project(n);
*this= *this*2 - orig;
}
 
// projects this vector onto v
void project(const Vector3<T> &v)
{ *this= v * (*this * v)/(v*v); }
 
// returns this vector projected onto v
Vector3<T> projected(const Vector3<T> &v)
{ return v * (*this * v)/(v*v); }
 
// computes the angle between 2 arbitrary vectors
T angle(const Vector3<T> &v1, const Vector3<T> &v2)
{ return (T)acosf((v1*v2) / (v1.length()*v2.length())); }
 
// computes the angle between 2 arbitrary normalized vectors
T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
{ return (T)acosf(v1*v2); }
 
// check if any elements are NAN
bool is_nan(void)
{ return isnan(x) || isnan(y) || isnan(z); }
 
// check if any elements are infinity
bool is_inf(void)
{ return isinf(x) || isinf(y) || isinf(z); }
 
// rotate by a standard rotation
void rotate(enum Rotation rotation);
 
};
 
typedef Vector3<int16_t> Vector3i;
typedef Vector3<uint16_t> Vector3ui;
typedef Vector3<int32_t> Vector3l;
typedef Vector3<uint32_t> Vector3ul;
typedef Vector3<float> Vector3f;
 
#endif // VECTOR3_H
/branches/dongfang_FC_rewrite/analog.c
2,117 → 2,87
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <stdio.h>
 
#include "analog.h"
#include "attitude.h"
#include "sensors.h"
#include "printf_P.h"
#include "mk3mag.h"
 
// for Delay functions
// for Delay functions used in calibration.
#include "timer0.h"
 
// For reading and writing acc. meter offsets.
#include "eeprom.h"
#include "debug.h"
 
// For debugOut
#include "output.h"
 
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
 
// TODO: Off to PROGMEM .
const char* recal = ", recalibration needed.";
 
/*
* For each A/D conversion cycle, each analog channel is sampled a number of times
* (see array channelsForStates), and the results for each channel are summed.
* Here are those for the gyros and the acc. meters. They are not zero-offset.
* They are exported in the analog.h file - but please do not use them! The only
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
* Gyro and accelerometer values for attitude computation.
* Unfiltered (this is unnecessary as noise should get absorbed in DCM).
* Normalized to rad/s.
* Data flow: ADCs (1 iteration) --> samplingADCData --offsetting--> gyro_attitude_tmp
* --rotation-->
* [filtering] --> gyro_attitude.
* Altimeter is also considered part of the "long" attitude loop.
*/
volatile uint16_t sensorInputs[8];
int16_t acc[3];
int16_t filteredAcc[3] = { 0,0,0 };
Vector3f gyro_attitude;
Vector3f accel;
 
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles.
* This stuff is for the aircraft control thread. It runs in unprocessed integers.
* (well some sort of scaling will be required).
* Data flow: ADCs (1 iteration) -> samplingADCData -> [offsetting and rotation] ->
* [filtering] --> gyro_control
*/
int16_t gyro_PID[2];
int16_t gyro_ATT[2];
int16_t gyro_control[3];
int16_t gyroD[2];
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
uint8_t gyroDWindowIdx = 0;
int16_t yawGyro;
int16_t magneticHeading;
uint8_t gyroDWindowIdx;
 
/*
* Air pressure. TODO: Might as well convert to floats / well known units.
*/
int32_t groundPressure;
int16_t dHeight;
 
uint32_t gyroActivity;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
 
sensorOffset_t gyroOffset;
sensorOffset_t accOffset;
sensorOffset_t accelOffset;
sensorOffset_t gyroAmplifierOffset;
 
/*
* In the MK coordinate system, nose-down is positive and left-roll is positive.
* If a sensor is used in an orientation where one but not both of the axes has
* an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
* Transform:
* pitch <- pp*pitch + pr*roll
* roll <- rp*pitch + rr*roll
* Not reversed, GYRO_QUADRANT:
* 0: pp=1, pr=0, rp=0, rr=1 // 0 degrees
* 1: pp=1, pr=-1,rp=1, rr=1 // +45 degrees
* 2: pp=0, pr=-1,rp=1, rr=0 // +90 degrees
* 3: pp=-1,pr=-1,rp=1, rr=1 // +135 degrees
* 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
* 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
* 6: pp=0, pr=1, rp=-1,rr=0 // +270 degrees
* 7: pp=1, pr=1, rp=-1,rr=1 // +315 degrees
* Reversed, GYRO_QUADRANT:
* 0: pp=-1,pr=0, rp=0, rr=1 // 0 degrees with pitch reversed
* 1: pp=-1,pr=-1,rp=-1,rr=1 // +45 degrees with pitch reversed
* 2: pp=0, pr=-1,rp=-1,rr=0 // +90 degrees with pitch reversed
* 3: pp=1, pr=-1,rp=-1,rr=1 // +135 degrees with pitch reversed
* 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
* 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
* 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed
* Redo this to that quadrant 0 is normal with an FC2.x.
*/
 
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
// Pitch to Pitch part
int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
// Roll to Pitch part
int8_t xy = rotationTab[(quadrant+2)%8];
// Pitch to Roll part
int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
// Roll to Roll part
int8_t yy = rotationTab[quadrant];
static const int8_t rotationTab[] = { 1, 1, 0, -1, -1, -1, 0, 1 };
// Pitch to Pitch part
int8_t xx = reverse ? rotationTab[(quadrant + 4) & 7] : rotationTab[quadrant]; // 1
// Roll to Pitch part
int8_t xy = rotationTab[(quadrant + 2) & 7]; // -1
// Pitch to Roll part
int8_t yx = reverse ? rotationTab[(quadrant + 2) & 7] : rotationTab[(quadrant + 6) & 7]; // -1
// Roll to Roll part
int8_t yy = rotationTab[quadrant]; // -1
 
int16_t xIn = result[0];
result[0] = xx*xIn + xy*result[1];
result[1] = yx*xIn + yy*result[1];
if (quadrant & 1) {
// A rotation was used above, where the factors were too large by sqrt(2).
// So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
// A suitable value for n: Sample is 11 bits. After transformation it is the sum
// of 2 11 bit numbers, so 12 bits. We have 4 bits left...
result[0] = (result[0]*11) >> 4;
result[1] = (result[1]*11) >> 4;
}
int16_t xIn = result[0];
int32_t tmp0, tmp1;
 
tmp0 = xx * xIn + xy * result[1];
tmp1 = yx * xIn + yy * result[1];
 
if (quadrant & 1) {
tmp0 = (tmp0 * 181L) >> 8;
tmp1 = (tmp1 * 181L) >> 8;
}
 
result[0] = (int16_t) tmp0;
result[1] = (int16_t) tmp1;
}
 
/*
121,7 → 91,6
volatile uint8_t rangewidth = 105;
 
// Direct from sensor, irrespective of range.
// volatile uint16_t rawAirPressure;
 
// Value of 2 samples, with range.
uint16_t simpleAirPressure;
143,7 → 112,7
int32_t airPressureSum;
 
// The number of samples summed into airPressureSum so far.
uint8_t pressureMeasurementCount;
uint8_t pressureSumCount;
 
/*
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
155,53 → 124,75
/*
* Control and status.
*/
volatile uint8_t analogDataReady = 1;
volatile uint16_t samplingADCData[8];
volatile uint16_t attitudeADCData[8];
 
/*
* Experiment: Measuring vibration-induced sensor noise.
*/
uint16_t gyroNoisePeak[3];
uint16_t accNoisePeak[3];
volatile uint8_t analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;
volatile uint8_t analog_attitudeDataStatus = ATTITUDE_SENSOR_NO_DATA;
// Number of ADC iterations done for current attitude loop.
volatile uint8_t attitudeSumCount;
 
volatile uint8_t adState;
volatile uint8_t ADCSampleCount;
volatile uint8_t adChannel;
 
// ADC channels
#define AD_GYRO_YAW 0
#define AD_GYRO_ROLL 1
#define AD_GYRO_PITCH 2
#define AD_AIRPRESSURE 3
#define AD_UBAT 4
#define AD_ACC_Z 5
#define AD_ACC_ROLL 6
#define AD_ACC_PITCH 7
 
/*
* Table of AD converter inputs for each state.
* The number of samples summed for each channel is equal to
* the number of times the channel appears in the array.
* The max. number of samples that can be taken in 2 ms is:
* 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
* loop needs a little time between reading AD values and
* re-enabling ADC, the real limit is (how much?) lower.
* The acc. sensor is sampled even if not used - or installed
* at all. The cost is not significant.
*/
const uint8_t channelsForStates[] PROGMEM = {
AD_GYRO_X,
AD_GYRO_Y,
AD_GYRO_Z,
 
const uint8_t channelsForStates[] PROGMEM = {
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW,
AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE,
AD_ACCEL_X,
AD_ACCEL_Y,
 
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc.
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
AD_ACC_PITCH, // at 12, finish pitch axis acc.
AD_ACC_ROLL, // at 13, finish roll axis acc.
AD_AIRPRESSURE, // at 14, finish air pressure.
AD_GYRO_PITCH, // at 15, finish pitch gyro
AD_GYRO_ROLL, // at 16, finish roll gyro
AD_UBAT // at 17, measure battery.
AD_GYRO_X,
AD_GYRO_Y,
//AD_GYRO_Z,
 
AD_ACCEL_Z,
AD_AIRPRESSURE,
 
AD_GYRO_X,
AD_GYRO_Y,
AD_GYRO_Z,
 
AD_ACCEL_X,
AD_ACCEL_Y,
 
AD_GYRO_X,
AD_GYRO_Y,
//AD_GYRO_Z,
 
//AD_ACCEL_Z,
//AD_AIRPRESSURE,
 
AD_GYRO_X,
AD_GYRO_Y,
AD_GYRO_Z,
 
AD_ACCEL_X,
AD_ACCEL_Y,
 
AD_GYRO_X,
AD_GYRO_Y,
//AD_GYRO_Z,
 
AD_ACCEL_Z,
AD_AIRPRESSURE,
 
AD_GYRO_Y,
AD_GYRO_X,
AD_GYRO_Z,
 
AD_ACCEL_X,
AD_ACCEL_Y,
 
AD_GYRO_X,
AD_GYRO_Y,
// AD_GYRO_Z,
 
//AD_ACCEL_Z,
//AD_AIRPRESSURE,
AD_UBAT
};
 
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
208,57 → 199,69
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
 
void analog_init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
 
//ADC0 ... ADC7 is connected to PortA pin 0 ... 7
DDRA = 0x00;
PORTA = 0x00;
// Digital Input Disable Register 0
// Disable digital input buffer for analog adc_channel pins
DIDR0 = 0xFF;
// external reference, adjust data to the right
ADMUX &= ~((1<<REFS1)|(1<<REFS0)|(1<<ADLAR));
// set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
ADMUX = (ADMUX & 0xE0);
//Set ADC Control and Status Register A
//Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
//Set ADC Control and Status Register B
//Trigger Source to Free Running Mode
ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
//ADC0 ... ADC7 is connected to PortA pin 0 ... 7
DDRA = 0x00;
PORTA = 0x00;
// Digital Input Disable Register 0
// Disable digital input buffer for analog adc_channel pins
DIDR0 = 0xFF;
// external reference, adjust data to the right
ADMUX &= ~((1 << REFS1) | (1 << REFS0) | (1 << ADLAR));
// set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
ADMUX = (ADMUX & 0xE0);
//Set ADC Control and Status Register A
//Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
ADCSRA = (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
//Set ADC Control and Status Register B
//Trigger Source to Free Running Mode
ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0));
 
for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
airPressureWindow[i] = 0;
}
for (uint8_t i = 0; i < MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
airPressureWindow[i] = 0;
}
 
windowedAirPressure = 0;
 
startAnalogConversionCycle();
startADCCycle();
 
// restore global interrupt flags
SREG = sreg;
// restore global interrupt flags
SREG = sreg;
}
 
uint16_t rawGyroValue(uint8_t axis) {
return sensorInputs[AD_GYRO_PITCH-axis];
// Convert axis number (X, Y, Z to ADC channel mapping (1, 2, 0)
uint16_t gyroValue(uint8_t axis, volatile uint16_t dataArray[]) {
switch (axis) {
case X:
return dataArray[AD_GYRO_X];
case Y:
return dataArray[AD_GYRO_Y];
case Z:
return dataArray[AD_GYRO_Z];
default:
return 0; // should never happen.
}
}
 
uint16_t rawAccValue(uint8_t axis) {
return sensorInputs[AD_ACC_PITCH-axis];
uint16_t gyroValueForFC13DACCalibration(uint8_t axis) {
return gyroValue(axis, samplingADCData);
}
 
void measureNoise(const int16_t sensor,
volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
if (sensor > (int16_t) (*noiseMeasurement)) {
*noiseMeasurement = sensor;
} else if (-sensor > (int16_t) (*noiseMeasurement)) {
*noiseMeasurement = -sensor;
} else if (*noiseMeasurement > damping) {
*noiseMeasurement -= damping;
} else {
*noiseMeasurement = 0;
}
// Convert axis number (X, Y, Z to ADC channel mapping (6, 7, 5)
uint16_t accValue(uint8_t axis, volatile uint16_t dataArray[]) {
switch (axis) {
case X:
return dataArray[AD_ACCEL_X];
case Y:
return dataArray[AD_ACCEL_Y];
case Z:
return dataArray[AD_ACCEL_Z];
default:
return 0; // should never happen.
}
}
 
/*
266,345 → 269,391
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
*/
uint16_t getSimplePressure(int advalue) {
uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
return result;
uint16_t result = (uint16_t) OCR0A * /*(uint16_t)*/ rangewidth + advalue;
result += (/*accel.z*/0 * (staticParams.airpressureAccZCorrection - 128)) >> 10;
return result;
}
 
void startAnalogConversionCycle(void) {
analogDataReady = 0;
 
// Stop the sampling. Cycle is over.
for (uint8_t i = 0; i < 8; i++) {
sensorInputs[i] = 0;
}
adState = 0;
adChannel = AD_GYRO_PITCH;
ADMUX = (ADMUX & 0xE0) | adChannel;
startADC();
void startADCCycle(void) {
for (uint8_t i=0; i<8; i++) {
samplingADCData[i] = 0;
}
ADCSampleCount = 0;
adChannel = AD_GYRO_X;
ADMUX = (ADMUX & 0xE0) | adChannel;
analog_controlDataStatus = CONTROL_SENSOR_SAMPLING_DATA;
J4HIGH;
startADC();
}
 
/*****************************************************
* Interrupt Service Routine for ADC
* Runs at 312.5 kHz or 3.2 �s. When all states are
* processed further conversions are stopped.
* Runs at 12 kHz. When all states are processed
* further conversions are stopped.
*****************************************************/
ISR(ADC_vect) {
sensorInputs[adChannel] += ADC;
// set up for next state.
adState++;
if (adState < sizeof(channelsForStates)) {
adChannel = pgm_read_byte(&channelsForStates[adState]);
// set adc muxer to next adChannel
ADMUX = (ADMUX & 0xE0) | adChannel;
// after full cycle stop further interrupts
startADC();
} else {
analogDataReady = 1;
// do not restart ADC converter.
}
ISR( ADC_vect) {
samplingADCData[adChannel] += ADC;
// set up for next state.
ADCSampleCount++;
if (ADCSampleCount < sizeof(channelsForStates)) {
adChannel = pgm_read_byte(&channelsForStates[ADCSampleCount]);
// set adc muxer to next adChannel
ADMUX = (ADMUX & 0xE0) | adChannel;
// after full cycle stop further interrupts
startADC();
} else {
J4LOW;
analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;
// do not restart ADC converter.
}
}
 
void measureGyroActivity(int16_t newValue) {
gyroActivity += newValue * newValue;
// abs(newValue); // (uint32_t)((int32_t)newValue * newValue);
/*
* Used in calibration only!
* Wait the specified number of millis, and then run a full sensor ADC cycle.
*/
void waitADCCycle(uint16_t delay) {
delay_ms(delay);
startADCCycle();
while(analog_controlDataStatus != CONTROL_SENSOR_DATA_READY)
;
}
 
#define GADAMPING 6
void dampenGyroActivity(void) {
static uint8_t cnt = 0;
void analog_updateControlData(void) {
/*
* 1) Near-saturation boost (dont bother with Z)
* 2) Offset
* 3) Rotation
* 4) Filter
* 5) Extract gyroD (should this be without near-saturation boost really? Ignore issue)
*/
 
if (++cnt >= IMUConfig.gyroActivityDamping) {
cnt = 0;
gyroActivity *= (uint32_t)((1L<<GADAMPING)-1);
gyroActivity >>= GADAMPING;
}
int16_t tempOffsetGyro[2];
int16_t tempGyro;
for (uint8_t axis=X; axis<=Y; axis++) {
tempGyro = gyroValue(axis, samplingADCData);
//debugOut.analog[3 + axis] = tempGyro;
//debugOut.analog[3 + 2] = gyroValue(Z, samplingADCData);
 
/*
if (gyroActivity >= 10) gyroActivity -= 10;
else if (gyroActivity <=- 10) gyroActivity += 10;
*/
}
/*
* Process the gyro data for the PID controller.
*/
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
// gyro with a wider range, and helps counter saturation at full control.
// There is hardly any reason to bother extrapolating yaw.
 
void analog_updateGyros(void) {
// for various filters...
int16_t tempOffsetGyro[2], tempGyro;
debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
for (uint8_t axis=0; axis<2; axis++) {
tempGyro = rawGyroValue(axis);
/*
* Process the gyro data for the PID controller.
*/
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
// gyro with a wider range, and helps counter saturation at full control.
if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
if (tempGyro < SENSOR_MIN_PITCHROLL) {
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
} else if (tempGyro > SENSOR_MAX_PITCHROLL) {
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
}
if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
if (tempGyro < SENSOR_MIN_XY) {
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
} else if (tempGyro > SENSOR_MAX_XY) {
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = (tempGyro - SENSOR_MAX_XY) * EXTRAPOLATION_SLOPE + SENSOR_MAX_XY;
}
}
 
// 2) Apply offset (rotation will take care of signs).
tempOffsetGyro[axis] = tempGyro - gyroOffset.offsets[axis];
}
 
// 2) Apply sign and offset, scale before filtering.
tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
}
// 2.1: Transform axes.
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_XY);
 
// 2.1: Transform axes.
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR);
for (uint8_t axis=X; axis<=Y; axis++) {
// Filter. There is no filter for Z and no need for one.
tempGyro = (gyro_control[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;
// 5) Differential measurement.
int16_t diff = tempGyro - gyro_control[axis];
gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
gyroD[axis] += diff;
gyroDWindow[axis][gyroDWindowIdx] = diff;
 
for (uint8_t axis=0; axis<2; axis++) {
// 3) Filter.
tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;
// 6) Done.
gyro_control[axis] = tempGyro;
}
 
// 4) Measure noise.
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
gyroDWindowIdx = 0;
}
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)
tempGyro = gyroOffset.offsets[Z] - gyroValue(Z, samplingADCData);
else
tempGyro = gyroValue(Z, samplingADCData) - gyroOffset.offsets[Z];
 
// 5) Differential measurement.
// gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis];
gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
gyroD[axis] += diff;
gyroDWindow[axis][gyroDWindowIdx] = diff;
gyro_control[Z] = tempGyro;
startADCCycle();
}
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro[axis];
/*
* The uint16s can take a max. of 1<<16-10) = 64 samples summed.
* Assuming a max oversampling count of 8 for the control loop, this is 8 control loop iterations
* summed. After 8 are reached, we just throw away all further data. This (that the attitude loop
* is more than 8 times slower than the control loop) should not happen anyway so there is no waste.
*/
#define MAX_OVEROVERSAMPLING_COUNT 8
 
// Prepare tempOffsetGyro for next calculation below...
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
}
void analog_sumAttitudeData(void) {
// From when this procedure completes, there is attitude data available.
if (analog_attitudeDataStatus == ATTITUDE_SENSOR_NO_DATA)
analog_attitudeDataStatus = ATTITUDE_SENSOR_DATA_READY;
 
/*
* Now process the data for attitude angles.
*/
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR);
 
dampenGyroActivity();
gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
if (analog_attitudeDataStatus == ATTITUDE_SENSOR_DATA_READY && attitudeSumCount < MAX_OVEROVERSAMPLING_COUNT) {
for (uint8_t i = 0; i < 8; i++) {
attitudeADCData[i] += samplingADCData[i];
}
attitudeSumCount++;
debugOut.analog[24] = attitudeSumCount;
}
}
 
/*
measureGyroActivity(tempOffsetGyro[PITCH]);
measureGyroActivity(tempOffsetGyro[ROLL]);
*/
measureGyroActivity(gyroD[PITCH]);
measureGyroActivity(gyroD[ROLL]);
void clearAttitudeData(void) {
for (uint8_t i = 0; i < 8; i++) {
attitudeADCData[i] = 0;
}
attitudeSumCount = 0;
analog_attitudeDataStatus = ATTITUDE_SENSOR_NO_DATA;
}
 
// We measure activity of yaw by plain gyro value and not d/dt, because:
// - There is no drift correction anyway
// - Effect of steady circular flight would vanish (it should have effect).
// int16_t diff = yawGyro;
// Yaw gyro.
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
void updateAttitudeVectors(void) {
/*
int16_t gyro_attitude_tmp[3];
Vector3f gyro_attitude;
Vector3f accel;
*/
 
// diff -= yawGyro;
// gyroD[YAW] -= gyroDWindow[YAW][gyroDWindowIdx];
// gyroD[YAW] += diff;
// gyroDWindow[YAW][gyroDWindowIdx] = diff;
int16_t tmpSensor[3];
 
// gyroActivity += (uint32_t)(abs(yawGyro)* IMUConfig.yawRateFactor);
measureGyroActivity(yawGyro);
// prevent gyro_attitude_tmp and attitudeSumCount from being updated.
// TODO: This only prevents interrupts from starting. Well its good enough really?
analog_attitudeDataStatus = ATTITUDE_SENSOR_READING_DATA;
 
if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
gyroDWindowIdx = 0;
}
}
tmpSensor[X] = gyroValue(X, attitudeADCData) - gyroOffset.offsets[X] * attitudeSumCount;
tmpSensor[Y] = gyroValue(Y, attitudeADCData) - gyroOffset.offsets[Y] * attitudeSumCount;
 
void analog_updateAccelerometers(void) {
// Pitch and roll axis accelerations.
for (uint8_t axis=0; axis<2; axis++) {
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
}
rotate(tmpSensor, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_XY);
 
rotate(acc, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_XY);
for(uint8_t axis=0; axis<3; axis++) {
filteredAcc[axis] = (filteredAcc[axis] * (IMUConfig.accFilterConstant - 1) + acc[axis]) / IMUConfig.accFilterConstant;
measureNoise(acc[axis], &accNoisePeak[axis], 1);
}
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)
tmpSensor[Z] = gyroOffset.offsets[Z] * attitudeSumCount - gyroValue(Z, attitudeADCData);
else
tmpSensor[Z] = gyroValue(Z, attitudeADCData) - gyroOffset.offsets[Z] * attitudeSumCount;
 
// Z acc.
if (IMUConfig.imuReversedFlags & 8)
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
gyro_attitude.x = ((float) tmpSensor[X]) / (GYRO_RATE_FACTOR_XY * attitudeSumCount);
gyro_attitude.y = ((float) tmpSensor[Y]) / (GYRO_RATE_FACTOR_XY * attitudeSumCount);
gyro_attitude.z = ((float) tmpSensor[Z]) / (GYRO_RATE_FACTOR_Z * attitudeSumCount);
 
// debugOut.analog[29] = acc[Z];
// Done with gyros. Now accelerometer:
tmpSensor[X] = accValue(X, attitudeADCData) - accelOffset.offsets[X] * attitudeSumCount;
tmpSensor[Y] = accValue(Y, attitudeADCData) - accelOffset.offsets[Y] * attitudeSumCount;
 
rotate(tmpSensor, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_XY);
 
// Z acc.
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z)
tmpSensor[Z] = accelOffset.offsets[Z] * attitudeSumCount - accValue(Z, attitudeADCData);
else
tmpSensor[Z] = accValue(Z, attitudeADCData) - accelOffset.offsets[Z] * attitudeSumCount;
 
// Blarh!!! We just skip acc filtering. There are trillions of samples already.
accel.x = (float)tmpSensor[X] / (ACCEL_FACTOR_XY * attitudeSumCount); // (accel.x + (float)tmpSensor[X] / (ACCEL_FACTOR_XY * attitudeSumCount)) / 2.0;
accel.y = (float)tmpSensor[Y] / (ACCEL_FACTOR_XY * attitudeSumCount); // (accel.y + (float)tmpSensor[Y] / (ACCEL_FACTOR_XY * attitudeSumCount)) / 2.0;
accel.z = (float)tmpSensor[Z] / (ACCEL_FACTOR_Z * attitudeSumCount); // (accel.z + (float)tmpSensor[Z] / (ACCEL_FACTOR_Z * attitudeSumCount)) / 2.0;
 
for (uint8_t i=0; i<3; i++) {
debugOut.analog[3 + i] = (int16_t)(gyro_attitude[i] * 100);
debugOut.analog[6 + i] = (int16_t)(accel[i] * 100);
}
}
 
void analog_updateAirPressure(void) {
static uint16_t pressureAutorangingWait = 25;
uint16_t rawAirPressure;
int16_t newrange;
// air pressure
if (pressureAutorangingWait) {
//A range switch was done recently. Wait for steadying.
pressureAutorangingWait--;
} else {
rawAirPressure = sensorInputs[AD_AIRPRESSURE];
if (rawAirPressure < MIN_RAWPRESSURE) {
// value is too low, so decrease voltage on the op amp minus input, making the value higher.
newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
if (newrange > MIN_RANGES_EXTRAPOLATION) {
pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
OCR0A = newrange;
} else {
if (OCR0A) {
OCR0A--;
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
}
}
} else if (rawAirPressure > MAX_RAWPRESSURE) {
// value is too high, so increase voltage on the op amp minus input, making the value lower.
// If near the end, make a limited increase
newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1;
if (newrange < MAX_RANGES_EXTRAPOLATION) {
pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
OCR0A = newrange;
} else {
if (OCR0A < 254) {
OCR0A++;
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
}
}
}
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near lower end of range. If the measurement saturates, the
// copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near upper end of range. If the measurement saturates, the
// copter may descend uncontrolledly... Simulate a drastic increase in pressure.
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
static uint16_t pressureAutorangingWait = 25;
uint16_t rawAirPressure;
int16_t newrange;
// air pressure
if (pressureAutorangingWait) {
//A range switch was done recently. Wait for steadying.
pressureAutorangingWait--;
} else {
// normal case.
// If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
// The 2 cases above (end of range) are ignored for this.
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
airPressureSum += simpleAirPressure;
}
// 2 samples were added.
pressureMeasurementCount += 2;
// Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...)
if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) {
rawAirPressure = attitudeADCData[AD_AIRPRESSURE] / attitudeSumCount;
if (rawAirPressure < MIN_RAWPRESSURE) {
// value is too low, so decrease voltage on the op amp minus input, making the value higher.
newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4);
if (newrange > MIN_RANGES_EXTRAPOLATION) {
pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
OCR0A = newrange;
} else {
if (OCR0A) {
OCR0A--;
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
}
}
} else if (rawAirPressure > MAX_RAWPRESSURE) {
// value is too high, so increase voltage on the op amp minus input, making the value lower.
// If near the end, make a limited increase
newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4);
if (newrange < MAX_RANGES_EXTRAPOLATION) {
pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
OCR0A = newrange;
} else {
if (OCR0A < 254) {
OCR0A++;
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
}
}
}
 
// The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
airPressureSum += simpleAirPressure >> 2;
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
if (simpleAirPressure < (uint16_t)(MIN_RANGES_EXTRAPOLATION * rangewidth)) {
// Danger: pressure near lower end of range. If the measurement saturates, the
// copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
} else if (simpleAirPressure > (uint16_t)(MAX_RANGES_EXTRAPOLATION * rangewidth)) {
// Danger: pressure near upper end of range. If the measurement saturates, the
// copter may descend uncontrolledly... Simulate a drastic increase in pressure.
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
} else {
// normal case.
// If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
// The 2 cases above (end of range) are ignored for this.
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
airPressureSum += simpleAirPressure;
}
 
uint32_t lastFilteredAirPressure = filteredAirPressure;
// 2 samples were added.
pressureSumCount += 2;
// Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 28...)
if (pressureSumCount == AIRPRESSURE_OVERSAMPLING) {
 
if (!staticParams.airpressureWindowLength) {
filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1)
+ airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant;
} else {
// use windowed.
windowedAirPressure += simpleAirPressure;
windowedAirPressure -= airPressureWindow[windowPtr];
airPressureWindow[windowPtr++] = simpleAirPressure;
if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
}
// The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
airPressureSum += simpleAirPressure >> 2;
 
// positive diff of pressure
int16_t diff = filteredAirPressure - lastFilteredAirPressure;
// is a negative diff of height.
dHeight -= diff;
// remove old sample (fifo) from window.
dHeight += dAirPressureWindow[dWindowPtr];
dAirPressureWindow[dWindowPtr++] = diff;
if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
pressureMeasurementCount = airPressureSum = 0;
uint32_t lastFilteredAirPressure = filteredAirPressure;
 
if (!staticParams.airpressureWindowLength) {
filteredAirPressure = (filteredAirPressure
* (staticParams.airpressureFilterConstant - 1)
+ airPressureSum
+ staticParams.airpressureFilterConstant / 2)
/ staticParams.airpressureFilterConstant;
} else {
// use windowed.
windowedAirPressure += simpleAirPressure;
windowedAirPressure -= airPressureWindow[windowPtr];
airPressureWindow[windowPtr++] = simpleAirPressure;
if (windowPtr >= staticParams.airpressureWindowLength)
windowPtr = 0;
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
}
 
// positive diff of pressure
int16_t diff = filteredAirPressure - lastFilteredAirPressure;
// is a negative diff of height.
dHeight -= diff;
// remove old sample (fifo) from window.
dHeight += dAirPressureWindow[dWindowPtr];
dAirPressureWindow[dWindowPtr++] = diff;
if (dWindowPtr >= staticParams.airpressureDWindowLength)
dWindowPtr = 0;
pressureSumCount = airPressureSum = 0;
}
}
}
debugOut.analog[25] = simpleAirPressure;
debugOut.analog[26] = OCR0A;
debugOut.analog[27] = filteredAirPressure;
}
 
void analog_updateBatteryVoltage(void) {
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
// This is divided by 3 --> 10.34 counts per volt.
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
// This is divided by 3 --> 10.34 counts per volt.
UBat = (3 * UBat + attitudeADCData[AD_UBAT] / 3) / 4;
}
 
void analog_update(void) {
analog_updateGyros();
analog_updateAccelerometers();
analog_updateAirPressure();
analog_updateBatteryVoltage();
#ifdef USE_MK3MAG
magneticHeading = volatileMagneticHeading;
#endif
void analog_updateAttitudeData(void) {
updateAttitudeVectors();
 
// TODO: These are waaaay off by now.
analog_updateAirPressure();
analog_updateBatteryVoltage();
 
clearAttitudeData();
}
 
void analog_setNeutral() {
gyro_init();
if (gyroOffset_readFromEEProm()) {
printf("gyro offsets invalid%s",recal);
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL;
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW;
}
if (accOffset_readFromEEProm()) {
printf("acc. meter offsets invalid%s",recal);
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
}
gyro_init();
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
for (uint8_t i=PITCH; i<=ROLL; i++) {
gyroNoisePeak[i] = 0;
gyroD[i] = 0;
for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
gyroDWindow[i][j] = 0;
}
}
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_with_adc_measurement(100, 0);
if (gyroOffset_readFromEEProm()) {
printf("gyro offsets invalid%s", recal);
gyroOffset.offsets[X] = gyroOffset.offsets[Y] = 512 * GYRO_OVERSAMPLING_XY;
gyroOffset.offsets[Z] = 512 * GYRO_OVERSAMPLING_Z;
// This will get the DACs for FC1.3 to offset to a reasonable value.
gyro_calibrate();
}
 
gyroActivity = 0;
if (accelOffset_readFromEEProm()) {
printf("acc. meter offsets invalid%s", recal);
accelOffset.offsets[X] = accelOffset.offsets[Y] = 512 * ACCEL_OVERSAMPLING_XY;
accelOffset.offsets[Z] = 512 * ACCEL_OVERSAMPLING_Z;
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {
accelOffset.offsets[Z] -= ACCEL_G_FACTOR_Z;
} else {
accelOffset.offsets[Z] += ACCEL_G_FACTOR_Z;
}
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
for (uint8_t i=X; i<=Y; i++) {
// gyroNoisePeak[i] = 0;
gyroD[i] = 0;
for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
gyroDWindow[i][j] = 0;
}
}
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
waitADCCycle(100);
}
 
void analog_calibrateGyros(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t offsets[3] = { 0, 0, 0 };
gyro_calibrate();
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
delay_ms_with_adc_measurement(10, 1);
for (axis = PITCH; axis <= YAW; axis++) {
offsets[axis] += rawGyroValue(axis);
#define GYRO_OFFSET_CYCLES 100
uint8_t i, axis;
int32_t offsets[3] = { 0, 0, 0 };
 
flightControlStatus = BLOCKED_FOR_CALIBRATION;
delay_ms(10);
 
gyro_calibrate();
 
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
waitADCCycle(5);
for (axis=X; axis<=Z; axis++) {
offsets[axis] += gyroValue(axis, samplingADCData);
}
}
}
for (axis = PITCH; axis <= YAW; axis++) {
gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
 
int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
}
for (axis=X; axis<=Z; axis++) {
gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES;
int16_t min = (512 - 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;
int16_t max = (512 + 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;
if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_X << axis;
}
 
gyroOffset_writeToEEProm();
startAnalogConversionCycle();
gyroOffset_writeToEEProm();
//startADCCycle();
}
 
/*
615,59 → 664,62
* directly from here, though.
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t offsets[3] = { 0, 0, 0 };
#define ACCEL_OFFSET_CYCLES 100
uint8_t i, axis;
int32_t offsets[3] = { 0, 0, 0 };
 
for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
delay_ms_with_adc_measurement(10, 1);
for (axis = PITCH; axis <= YAW; axis++) {
offsets[axis] += rawAccValue(axis);
flightControlStatus = BLOCKED_FOR_CALIBRATION;
delay_ms(10);
 
for (i = 0; i < ACCEL_OFFSET_CYCLES; i++) {
waitADCCycle(5);
for (axis=X; axis<=Z; axis++) {
offsets[axis] += accValue(axis, samplingADCData);
}
}
}
 
for (axis = PITCH; axis <= YAW; axis++) {
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
int16_t min,max;
if (axis==Z) {
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) {
// TODO: This assumes a sensitivity of +/- 2g.
min = (256-200) * ACC_OVERSAMPLING_Z;
max = (256+200) * ACC_OVERSAMPLING_Z;
} else {
// TODO: This assumes a sensitivity of +/- 2g.
min = (768-200) * ACC_OVERSAMPLING_Z;
max = (768+200) * ACC_OVERSAMPLING_Z;
}
for (axis=X; axis<=Z; axis++) {
accelOffset.offsets[axis] = (offsets[axis] + ACCEL_OFFSET_CYCLES / 2) / ACCEL_OFFSET_CYCLES;
int16_t min, max;
if (axis == Z) {
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {
// TODO: This assumes a sensitivity of +/- 2g.
min = (256 - 200) * ACCEL_OVERSAMPLING_Z;
max = (256 + 200) * ACCEL_OVERSAMPLING_Z;
} else {
// TODO: This assumes a sensitivity of +/- 2g.
min = (768 - 200) * ACCEL_OVERSAMPLING_Z;
max = (768 + 200) * ACCEL_OVERSAMPLING_Z;
}
} else {
min = (512 - 200) * ACCEL_OVERSAMPLING_XY;
max = (512 + 200) * ACCEL_OVERSAMPLING_XY;
}
if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
versionInfo.hardwareErrors[0] |= FC_ERROR0_ACCEL_X << axis;
}
}
 
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {
accelOffset.offsets[Z] -= ACCEL_G_FACTOR_Z;
} else {
min = (512-200) * ACC_OVERSAMPLING_XY;
max = (512+200) * ACC_OVERSAMPLING_XY;
accelOffset.offsets[Z] += ACCEL_G_FACTOR_Z;
}
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis;
}
}
 
accOffset_writeToEEProm();
startAnalogConversionCycle();
accelOffset_writeToEEProm();
// startADCCycle();
}
 
void analog_setGround() {
groundPressure = filteredAirPressure;
groundPressure = filteredAirPressure;
}
 
int32_t analog_getHeight(void) {
return groundPressure - filteredAirPressure;
int32_t height = groundPressure - filteredAirPressure;
debugOut.analog[28] = height;
return height;
}
 
int16_t analog_getDHeight(void) {
/*
int16_t result = 0;
for (int i=0; i<staticParams.airpressureDWindowLength; i++) {
result -= dAirPressureWindow[i]; // minus pressure is plus height.
}
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
return result;
*/
return dHeight;
return dHeight;
}
/branches/dongfang_FC_rewrite/analog.h
2,102 → 2,99
#define _ANALOG_H
#include <inttypes.h>
#include "configuration.h"
#include "Vector3.h"
 
/*
About setting constants for different gyros:
Main parameters are positive directions and voltage/angular speed gain.
The "Positive direction" is the rotation direction around an axis where
the corresponding gyro outputs a voltage > the no-rotation voltage.
A gyro is considered, in this code, to be "forward" if its positive
direction is:
- Nose down for pitch
- Left hand side down for roll
- Clockwise seen from above for yaw.
Setting gyro gain correctly: All sensor measurements in analog.c take
place in a cycle, each cycle comprising all sensors. Some sensors are
sampled more than once (oversampled), and the results added.
In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
or 4 (other versions), offset to zero, low pass filtered and then assigned
to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
*/
#define GYRO_FACTOR_PITCHROLL 1
 
/*
GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
ADCValue [units] =
rotational speed [deg/s] *
gyro sensitivity [V / deg/s] *
rotational speed [rad/s] *
gyro sensitivity [V/rad/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
 
GYRO_HW_FACTOR is:
gyro sensitivity [V / deg/s] *
gyro sensitivity [V/rad/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
 
Examples:
FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
FC1.3 has 38.39 mV/rad/s gyros and amplifiers with a gain of 5.7:
GYRO_HW_FACTOR = 0.03839 V/deg/s*5.7*1024/3V = 74.688 units/rad/s.
 
FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
FC2.0 has 6*(3/5) mV/rad/s gyros (they are ratiometric) and no amplifiers:
GYRO_HW_FACTOR = 0.006 V/rad/s*1*1024*3V/5V/3V = 70.405 units/(rad/s).
 
My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
(only about half as sensitive as V1.3. But it will take about twice the
rotation rate!)
GYRO_HW_FACTOR = 0.002 V/rad/s*1*1024/3V = 39.1139 units/(rad/s)
(only about half as sensitive as V1.3. But it will take about twice the rotation rate!)
 
GYRO_HW_FACTOR is given in the makefile.
*/
 
#define ACCEL_HW_FACTOR 204.8f
 
// If defined, acceleration is scaled to m/s^2. Else it is scaled to g's.
#define USE_MSSQUARED 1
 
/*
* How many samples are added in one ADC loop, for pitch&roll and yaw,
* respectively. This is = the number of occurences of each channel in the
* channelsForStates array in analog.c.
*/
#define GYRO_OVERSAMPLING_PITCHROLL 4
#define GYRO_OVERSAMPLING_YAW 2
#define GYRO_OVERSAMPLING_XY 8
#define GYRO_RATE_FACTOR_XY (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_XY * GYRO_XY_CORRECTION)
#define GYRO_RATE_FACTOR_XY_MS (GYRO_RATE_FACTOR_XY * 1000.0)
 
#define ACC_OVERSAMPLING_XY 2
#define ACC_OVERSAMPLING_Z 1
#define GYRO_OVERSAMPLING_Z 4
#define GYRO_RATE_FACTOR_Z (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_Z * GYRO_Z_CORRECTION)
#define GYRO_RATE_FACTOR_Z_MS (GYRO_RATE_FACTOR_Z * 1000.0)
 
/*
* The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
*/
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
#define ACCEL_OVERSAMPLING_XY 4
#define ACCEL_G_FACTOR_XY (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_XY)
#define ACCEL_M_SSQUARED_FACTOR_XY (ACCEL_G_FACTOR_XY / 9.82f)
 
/*
* The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
* Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
*/
#define ACCEL_OVERSAMPLING_Z 2
 
// number of counts per g
#define ACCEL_G_FACTOR_Z (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_Z)
// number of counts per m/s^2
#define ACCEL_M_SSQUARED_FACTOR_Z (ACCEL_G_FACTOR_Z / 9.82f)
 
#ifdef USE_MSSQUARED
#define ACCEL_FACTOR_XY ACCEL_M_SSQUARED_FACTOR_XY
#define ACCEL_FACTOR_Z ACCEL_M_SSQUARED_FACTOR_Z
#else
#define ACCEL_FACTOR_XY ACCEL_G_FACTOR_XY
#define ACCEL_FACTOR_Z ACCEL_G_FACTOR_Z
#endif
/*
* Gyro saturation prevention.
*/
// How far from the end of its range a gyro is considered near-saturated.
#define SENSOR_MIN_PITCHROLL 32
#define SENSOR_MIN_XY (32 * GYRO_OVERSAMPLING_XY)
// Other end of the range (calculated)
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
#define SENSOR_MAX_XY (GYRO_OVERSAMPLING_XY * 1023 - SENSOR_MIN_XY)
// Max. boost to add "virtually" to gyro signal at total saturation.
#define EXTRAPOLATION_LIMIT 2500
// Slope of the boost (calculated)
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_XY)
 
/*
* This value is subtracted from the gyro noise measurement in each iteration,
* making it return towards zero.
*/
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
#define X 0
#define Y 1
#define Z 2
 
#define PITCH 0
#define ROLL 1
#define YAW 2
#define Z 2
// ADC channels
#define AD_GYRO_Z 0
#define AD_GYRO_X 1
#define AD_GYRO_Y 2
#define AD_AIRPRESSURE 3
#define AD_UBAT 4
#define AD_ACCEL_Z 5
#define AD_ACCEL_Y 6
#define AD_ACCEL_X 7
 
/*
* The values that this module outputs
* These first 2 exported arrays are zero-offset. The "PID" ones are used
107,11 → 104,18
* in filtering, and when a gyro becomes near saturated.
* Maybe this distinction is not really necessary.
*/
extern int16_t gyro_PID[2];
extern int16_t gyro_ATT[2];
extern Vector3f gyro_attitude;
 
/*
* The acceleration values that this module outputs. They are zero based.
*/
extern Vector3f accel;
 
#define GYRO_D_WINDOW_LENGTH 8
 
extern int16_t gyro_control[3];
extern int16_t gyroD[2];
extern int16_t yawGyro;
 
extern int16_t UBat;
 
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
118,28 → 122,22
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
 
extern sensorOffset_t gyroOffset;
extern sensorOffset_t accOffset;
extern sensorOffset_t accelOffset;
extern sensorOffset_t gyroAmplifierOffset;
 
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
//extern volatile int16_t rawGyroSum[3];
typedef enum {
CONTROL_SENSOR_SAMPLING_DATA,
CONTROL_SENSOR_DATA_READY,
} ControlSensorDataStatus;
 
/*
* The acceleration values that this module outputs. They are zero based.
*/
extern int16_t acc[3];
extern int16_t filteredAcc[3];
// extern volatile int32_t stronglyFilteredAcc[3];
typedef enum {
ATTITUDE_SENSOR_NO_DATA,
ATTITUDE_SENSOR_DATA_READY,
ATTITUDE_SENSOR_READING_DATA
} AttitudeSensorDataStatus;
 
/*
* Diagnostics: Gyro noise level because of motor vibrations. The variables
* only really reflect the noise level when the copter stands still but with
* its motors running.
*/
extern uint16_t gyroNoisePeak[3];
extern uint16_t accNoisePeak[3];
extern volatile uint8_t analog_controlDataStatus;
extern volatile uint8_t analog_attitudeDataStatus;
 
/*
* Air pressure.
171,7 → 169,7
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
*/
 
#define AIRPRESSURE_OVERSAMPLING 14
#define AIRPRESSURE_OVERSAMPLING 28
#define AIRPRESSURE_FILTER 8
// Minimum A/D value before a range change is performed.
#define MIN_RAWPRESSURE (200 * 2)
186,22 → 184,6
 
#define ABS_ALTITUDE_OFFSET 108205
 
extern uint16_t simpleAirPressure;
/*
* At saturation, the filteredAirPressure may actually be (simulated) negative.
*/
extern int32_t filteredAirPressure;
 
extern int16_t magneticHeading;
 
extern uint32_t gyroActivity;
 
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
*/
extern volatile uint8_t analogDataReady;
 
 
void analog_init(void);
 
/*
208,17 → 190,19
* This is really only for use for the ENC-03 code module, which needs to get the raw value
* for its calibration. The raw value should not be used for anything else.
*/
uint16_t rawGyroValue(uint8_t axis);
uint16_t gyroValueForFC13DACCalibration(uint8_t axis);
 
/*
* Start the conversion cycle. It will stop automatically.
*/
void startAnalogConversionCycle(void);
void startADCCycle(void);
void waitADCCycle(uint16_t delay);
 
/*
* Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
*/
void analog_update(void);
void analog_updateControlData(void);
void analog_sumAttitudeData(void);
void analog_updateAttitudeData(void);
 
/*
* Read gyro and acc.meter calibration from EEPROM.
237,7 → 221,6
 
 
void analog_setGround(void);
 
int32_t analog_getHeight(void);
int16_t analog_getDHeight(void);
 
/branches/dongfang_FC_rewrite/attitude.h
6,141 → 6,23
#define _ATTITUDE_H
 
#include <inttypes.h>
#include <math.h>
 
#include "analog.h"
#include "timer0.h"
//#include "timer0.h"
//#define _PI 3.1415926535897932384626433832795
 
/*
* If you have no acc. sensor or do not want to use it, remove this define. This will cause the
* acc. sensor to be ignored at attitude calibration.
*/
#define ATTITUDE_USE_ACC_SENSORS yes
#define ACC_MS2_FACTOR (1.0f / ACC_G_FACTOR_XY)
 
/*
* Default hysteresis to use for the -180 degree to 180 degree wrap.
*/
#define PITCHOVER_HYSTERESIS 0L
#define ROLLOVER_HYSTERESIS 0L
// 57.3
#define RAD_DEG_FACTOR (180.0 / M_PI)
 
/*
* The frequency at which numerical integration takes place. 488 in original code.
*/
#define INTEGRATION_FREQUENCY F_MAINLOOP
 
/*
* Gyro readings are divided by this before being used in attitude control. This will scale them
* to match the scale of the stick control etc. variables. This is just a rough non-precision
* scaling - the below definitions make precise conversion factors.
*/
#define HIRES_GYRO_INTEGRATION_FACTOR 1
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
 
/*
Gyro integration:
 
The control loop executes at INTEGRATION_FREQUENCY Hz, and for each iteration
gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
Assuming a constant rotation rate v and a zero initial gyroIntegral
(for this explanation), we get:
 
gyroIntegral =
t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
 
For one degree of rotation: t*v = 1:
 
gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
 
This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.
 
Examples:
FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545
FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399
My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333
*/
//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
 
/*
* Constant for deriving an attitude angle from acceleration measurement.
*
* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
* In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
* The sine of v is the far side / the hypothenusis:
* sin v = acc / sqrt(acc^2 + acc_z^2)
* Using that v is a small angle, and the near side is about equal to the the hypothenusis:
* sin v ~= acc / acc_z
* Assuming that the multicopter is hovering at small pitch and roll angles, acc_z is about 410,
* and sin v ~= v (small angles, in radians):
* sin v ~= acc / 410
* v / 57.3 ~= acc / 410
* v ~= acc * 57.3 / 410
* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
*
* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
*/
#define DEG_ACC_FACTOR 7
 
 
/*
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
* = the factor an acc. sensor should be multiplied by to get the gyro integral
* value for the same (small) angle.
* The value is about 200.
*/
#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
 
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
#define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L)
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)
 
/*
* Rotation rates
*/
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
extern int16_t differential[2];
 
/*
* Attitudes calculated by numerical integration of gyro rates
*/
extern int32_t attitude[2];
extern int16_t attitude[3];
 
// This is really a flight module thing, but it should be corrected along
// when the yaw angle is corrected from the compass, and that happens here.
// extern int32_t yawAngleDiff;
 
/*
* Compass navigation
*/
extern int32_t heading;
extern uint16_t ignoreCompassTimer;
extern uint16_t accVector;
 
extern int32_t headingError;
 
 
/*
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves
* can be updated in flight by different ways, for example:
* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
* - Summing up how much acc. meter correction was done to the gyro integrals over the last n
* integration, and then adding the sum / n to the dynamic offset
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
* - Invent your own...
*/
extern int16_t dynamicOffset[2], dynamicOffsetYaw;
 
/*
* For NaviCtrl use.
*/
extern int16_t averageAcc[2], averageAccCount;
 
/*
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
*/
147,18 → 29,8
void attitude_setNeutral(void);
 
/*
* Experiment.
*/
// void attitude_startDynamicCalibration(void);
// void attitude_continueDynamicCalibration(void);
 
int32_t getAngleEstimateFromAcc(uint8_t axis);
 
/*
* Main routine, called from the flight loop.
*/
void calculateFlightAttitude(void);
void attitude_update(void);
 
void attitude_resetHeadingToMagnetic(void);
 
#endif //_ATTITUDE_H
/branches/dongfang_FC_rewrite/attitude_ardupilot.c
0,0 → 1,47
#include <stdlib.h>
#include <avr/io.h>
#include <stdio.h>
 
#include "attitude.h"
#include "configuration.h"
#include <avr/interrupt.h>
#include "timer0.h"
#include "debug.h"
 
// where our main data flow comes from.
#include "analog.h"
#include "AP_AHRS_DCM.h"
#include "AP_GPS.h"
 
int16_t attitude[3];
 
//AP_Compass_HIL compass;
GPS* gps;
AP_AHRS_DCM ahrs(gps);
 
uint8_t imu_sequence = 0; //incremented on each call to imu_update
 
/************************************************************************
* Neutral Readings
************************************************************************/
void attitude_setNeutral(void) {
analog_setNeutral();
ahrs.reset();
}
 
void attitude_update(void) {
static uint32_t timestampJiffies;
if (analog_attitudeDataStatus == ATTITUDE_SENSOR_DATA_READY) {
J3TOGGLE;
// OOPS: The attitude data might get added to while this is running...
// debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
analog_updateAttitudeData();
cli();
float jiffies = jiffiesClock - timestampJiffies;
timestampJiffies = jiffiesClock;
sei();
ahrs.update(attitude, jiffies * T_TIMER0IRQ);
} else {
// debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
}
}
/branches/dongfang_FC_rewrite/attitude_starlino.c
0,0 → 1,217
#include <stdlib.h>
#include <avr/io.h>
#include <stdio.h>
 
#include "attitude.h"
#include "commands.h"
#include "vector3d.h"
// For scope debugging only!
#include "rc.h"
 
// where our main data flow comes from.
#include "analog.h"
 
#include "configuration.h"
#include "output.h"
 
// Some calculations are performed depending on some stick related things.
#include "controlMixer.h"
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
* Gyro integrals. These are the rotation angles of the airframe relative to horizontal.
*/
float attitude[3];
 
uint8_t imu_sequence = 0; //incremented on each call to imu_update
 
float dcmAcc[3][3]; //dcm matrix according to accelerometer
float dcmGyro[3][3]; //dcm matrix according to gyroscopes
float dcmEst[3][3]; //estimated dcm matrix by fusion of accelerometer and gyro
 
float getAngleEstimateFromAcc(uint8_t axis) {
return GYRO_ACC_FACTOR * acc_ATT[axis];
}
 
/************************************************************************
* Neutral Readings
************************************************************************/
void attitude_setNeutral(void) {
// Calibrate hardware.
analog_setNeutral();
unsigned char i, j;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
dcmGyro[i][j] = (i == j) ? 1.0 : 0.0;
}
 
/*
How to use this module in other projects.
 
Input variables are:
adcAvg[0..6] ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update
 
Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis
 
First you must initialize the module with:
imu_init();
 
Then call periodically every 5-20ms:
imu_update(interval_us);
it is assumed that you also update periodicall the adcAvg[0..5] array
 
*/
 
#define ACC_WEIGHT_MAX 0.02 //maximum accelerometer weight in accelerometer-gyro fusion formula
//this value is tuned-up experimentally: if you get too much noise - decrease it
//if you get a delayed response of the filtered values - increase it
//starting with a value of 0.01 .. 0.05 will work for most sensors
 
#define ACC_ERR_MAX 0.3 //maximum allowable error(external acceleration) where accWeight becomes 0
 
//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------
 
//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
//Note: dcm and dcmResult can be the same
void dcm_orthonormalize(float dcm[3][3]) {
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*) (dcm[0]), (float*) (dcm[1]));
float delta[2][3];
vector3d_scale(-err / 2, (float*) (dcm[1]), (float*) (delta[0]));
vector3d_scale(-err / 2, (float*) (dcm[0]), (float*) (delta[1]));
vector3d_add((float*) (dcm[0]), (float*) (delta[0]), (float*) (dcm[0]));
vector3d_add((float*) (dcm[1]), (float*) (delta[0]), (float*) (dcm[1]));
 
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*) (dcm[0]), (float*) (dcm[1]), (float*) (dcm[2]));
//re-nomralization
vector3d_normalize((float*) (dcm[0]));
vector3d_normalize((float*) (dcm[1]));
vector3d_normalize((float*) (dcm[2]));
}
 
//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]) {
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
 
int i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for (i = 0; i < 3; i++) {
vector3d_cross(w, dcm[i], dR);
vector3d_add(dcm[i], dR, dcm[i]);
}
 
//make matrix orthonormal again
dcm_orthonormalize(dcm);
}
 
//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
#define ACC_WEIGHT 0.01 //accelerometer data weight relative to gyro's weight of 1
#define MAG_WEIGHT 0.0 //magnetometer data weight relative to gyro's weight of 1
 
void imu_update(void) {
int i;
imu_sequence++;
 
//interval since last call
 
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
 
 
//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
float Kacc[3];
//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc[0] = -acc_ATT[X];
Kacc[1] = -acc_ATT[Y];
Kacc[2] = -acc_ATT[Z];
vector3d_normalize(Kacc);
//calculate correction vector to bring dcmGyro's K vector closer to Acc vector (K vector according to accelerometer)
float wA[3];
vector3d_cross(dcmGyro[2], Kacc, wA); // wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro
 
//---------------
//Magnetomer
//---------------
//calculate correction vector to bring dcmGyro's I vector closer to Mag vector (I vector according to magnetometer)
float Imag[3];
float wM[3];
//in the absense of magnetometer let's assume North vector (I) is always in XZ plane of the device (y coordinate is 0)
Imag[0] = sqrt(1 - dcmGyro[0][2] * dcmGyro[0][2]);
Imag[1] = 0;
Imag[2] = dcmGyro[0][2];
 
vector3d_cross(dcmGyro[0], Imag, wM); // wM = Igyro x Imag, roation needed to bring Imag to Igyro
 
//---------------
//dcmGyro
//---------------
float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -gyro_ATT[PITCH] / 1000.0; //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -gyro_ATT[ROLL] / 1000.0; //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -gyro_ATT[YAW] / 1000.0; //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
 
for (i = 0; i < 3; i++) {
w[i] *= INTEGRATION_TIME_MS; //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + ACC_WEIGHT * wA[i] + MAG_WEIGHT * wM[i]) / (1.0
+ ACC_WEIGHT + MAG_WEIGHT);
}
 
dcm_rotate(dcmGyro, w);
 
//Output for PicQuadController_GYRO_DEBUG1.scc
//only output data ocasionally to allow computer to process data
/*
if(0 == imu_sequence % 4){
printf("%.5f,",(double)interval_ms);
print_float_list(3,(float*)w);
printf(",%.2f,%.2f,%.2f",adcAvg[3+1],adcAvg[3+0],adcAvg[3+2]);
 
printf("\n ");
}
*/
 
//Output for: PICQUADCONTROLLER_DEBUG1.pde
//only output data ocasionally to allow computer to process data
/*
if (0 == imu_sequence % 16) {
printf("%.2f,", (double) interval_ms);
print_float_list(3, (float*) Kacc);
printf(", ");
print_float_list(9, (float*) dcmGyro);
printf("\n");
}
*/
}
 
void attitude_update(void) {
analog_update();
startAnalogConversionCycle();
 
// Takes 4 ms.
imu_update();
}
/branches/dongfang_FC_rewrite/beeper.h
0,0 → 1,19
#ifndef _BEEPER_H
#define _BEEPER_H
 
#define BEEP_MODULATION_NONE 0xFFFF
#define BEEP_MODULATION_RCALARM 0x0C00
#define BEEP_MODULATION_I2CALARM 0x0080
#define BEEP_MODULATION_BATTERYALARM 0x0300
#define BEEP_MODULATION_EEPROMALARM 0x0007
 
extern volatile uint16_t beepModulation;
extern volatile uint16_t beepTime;
 
void beep(uint16_t millis);
void beepNumber(uint8_t numbeeps);
void beepRCAlarm(void);
void beepI2CAlarm(void);
void beepBatteryAlarm(void);
 
#endif
/branches/dongfang_FC_rewrite/commands.c
3,89 → 3,89
#include "controlMixer.h"
#include "flight.h"
#include "eeprom.h"
#include "analog.h"
#include "attitude.h"
#include "output.h"
#include "rc.h"
#include "configuration.h"
#include "beeper.h"
 
#ifdef USE_MK3MAG
// TODO: Kick that all outa here!
uint8_t compassCalState = 0;
uint8_t compassCalState;
#endif
 
void commands_handleCommands(void) {
/*
* Get the current command (start/stop motors, calibrate), if any.
*/
uint8_t command = controlMixer_getCommand();
uint8_t repeated = controlMixer_isCommandRepeated();
uint8_t argument = controlMixer_getArgument();
/*
* Get the current command (start/stop motors, calibrate), if any.
*/
uint8_t command = controlMixer_getCommand();
uint8_t repeated = controlMixer_isCommandRepeated();
 
if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
if (command == COMMAND_GYROCAL && !repeated) {
// Run gyro calibration but do not repeat it.
// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
// isFlying = 0;
// check roll/pitch stick position
// if pitch stick is top or roll stick is left or right --> change parameter setting
// according to roll/pitch stick position
if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
uint8_t argument = controlMixer_getArgument();
if ((command==COMMAND_GYROCAL || command==COMMAND_GYRO_ACC_CAL) && !repeated) {
// Run gyro calibration but do not repeat it.
// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
// isFlying = 0;
// check roll/pitch stick position
// if pitch stick is top or roll stick is left or right --> change parameter setting
// according to roll/pitch stick position
 
if (argument < 6) {
// Gyro calinbration, with or without selecting a new parameter-set.
if (argument > 0 && argument < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(argument);
}
paramSet_readFromEEProm(getActiveParamSet());
analog_calibrateGyros();
attitude_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
if (argument < 6) {
// Gyro calinbration, with or without selecting a new parameter-set.
if (argument > 0 && argument < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(argument);
}
paramSet_readFromEEProm(getActiveParamSet());
analog_calibrateGyros();
attitude_setNeutral();
controlMixer_setNeutral();
flight_setGround();
beepNumber(getActiveParamSet());
}
#ifdef USE_MK3MAG
else if ((staticParams.bitConfig & CFG_COMPASS_ENABLED) && argument == 7) {
// If right stick is centered and down
compassCalState = 1;
beep(1000);
}
else if ((staticParams.bitConfig & CFG_COMPASS_ENABLED) && argument == 7) {
// If right stick is centered and down
compassCalState = 1;
beep(1000);
}
#endif
}
}
 
// save the ACC neutral setting to eeprom
else {
if (command == COMMAND_ACCCAL && !repeated) {
// Run gyro and acc. meter calibration but do not repeat it.
analog_calibrateAcc();
attitude_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
}
} // end !MOTOR_RUN condition.
if (command == COMMAND_START) {
isFlying = 1; // TODO: Really????
// if (!controlMixer_isCommandRepeated()) {
// attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all???
// } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
// attitude_continueDynamicCalibration();
// setPointYaw = 0;
// IPartPitch = 0;
// IPartRoll = 0;
// }
} else if (command == COMMAND_STOP) {
isFlying = 0;
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
// save the ACC neutral setting to eeprom
if ((command==COMMAND_ACCCAL || command==COMMAND_GYRO_ACC_CAL) && !repeated) {
// Run gyro and acc. meter calibration but do not repeat it.
analog_calibrateAcc();
attitude_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
} // end !MOTOR_RUN condition.
if (command == COMMAND_START) {
isFlying = 1; // TODO: Really????
// if (!controlMixer_isCommandRepeated()) {
// attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
MKFlags |= MKFLAG_MOTOR_RUN;
// } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
// attitude_continueDynamicCalibration();
// setPointYaw = 0;
// IPartPitch = 0;
// IPartRoll = 0;
// }
} else if (command == COMMAND_STOP) {
isFlying = 0;
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
}
 
/*
* if (controlMixer_testCompassCalState()) {
compassCalState++;
if (compassCalState < 5)
beepNumber(compassCalState);
else
beep(1000);
}
compassCalState++;
if (compassCalState < 5)
beepNumber(compassCalState);
else
beep(1000);
}
*
*/
 
/branches/dongfang_FC_rewrite/commands.h
6,11 → 6,12
* An enumeration over the start motors, stop motors, calibrate gyros
* and calibreate acc. meters commands.
*/
#define COMMAND_NONE 0
#define COMMAND_START 6
#define COMMAND_STOP 8
#define COMMAND_GYROCAL 2
#define COMMAND_ACCCAL 4
#define COMMAND_NONE 0
#define COMMAND_GYROCAL 1
#define COMMAND_ACCCAL 2
#define COMMAND_GYRO_ACC_CAL 3
#define COMMAND_STOP 10
#define COMMAND_START 11
 
extern uint8_t compassCalState;
 
/branches/dongfang_FC_rewrite/compassControl.c
2,6 → 2,7
#include "controlMixer.h"
#include "attitude.h"
#include "compassControl.h"
#include "definitions.h"
#include <stdlib.h>
 
// 4 modes of control (like with the simple height controller):
14,21 → 15,22
// The target heading variable is stored here (not in the flight or attitude modules), as here
// is where the regulation takes place. Also, it was found that a target heading was difficult
// to maintain in the flight module and the flight module would sometimes need to write back
// to it. The only possible (but sufficient) output from here is PRTY[CONTROL_YAW].
// to it. The only possible (but sufficient) output from here is RPTY[CONTROL_YAW].
 
int32_t navigationTargetHeading;
int32_t magneticTargetHeading;
int32_t bending = 0;
int32_t bending;
 
void compass_setTakeoffHeading(int32_t heading) {
magneticTargetHeading = heading;
}
 
void CC_periodicTaskAndPRTY(int16_t* PRTY) {
int16_t currentYaw = PRTY[CONTROL_YAW];
void CC_periodicTaskAndRPTY(int16_t* RPTY) {
int16_t currentYaw = RPTY[CONTROL_YAW];
 
switch (staticParams.compassMode) {
case COMPASS_MODE_OFF:
default:
bending = 0;
break;
case COMPASS_MODE_TAKEOFF:
61,6 → 63,4
// We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID).
// Bending blends in like: (magneticTargetHeading +- bending - heading)
}
 
 
}
/branches/dongfang_FC_rewrite/compassControl.h
18,6 → 18,6
extern int32_t navigationTargetHeading;
 
void compass_setTakeoffHeading(int32_t heading);
void CC_periodicTaskAndPRTY(int16_t* PRTY);
void CC_periodicTaskAndRPTY(int16_t* RPTY);
 
#endif
/branches/dongfang_FC_rewrite/configuration.c
6,33 → 6,44
#include "rc.h"
#include "output.h"
#include "flight.h"
#include "debug.h"
 
int16_t variables[VARIABLE_COUNT];
ParamSet_t staticParams;
ChannelMap_t channelMap;
MotorMixer_t motorMixer;
OutputMixer_t outputMixer;
IMUConfig_t IMUConfig;
volatile DynamicParams_t dynamicParams;
 
uint8_t CPUType;
uint8_t boardRelease;
uint8_t requiredMotors;
 
VersionInfo_t versionInfo;
 
// MK flags. TODO: Replace by enum. State machine.
uint16_t isFlying = 0;
volatile uint8_t MKFlags = 0;
uint16_t isFlying;
volatile uint8_t MKFlags;
 
const MMXLATION XLATIONS[] = {
{offsetof(ParamSet_t, flightMode), offsetof(DynamicParams_t, flightMode),0,255},
 
{offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255},
{offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255},
{offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255},
{offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255},
{offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255},
 
{offsetof(ParamSet_t, attGyroP), offsetof(DynamicParams_t, attGyroP),0,250},
{offsetof(ParamSet_t, attGyroI), offsetof(DynamicParams_t, attGyroI),0,250},
{offsetof(ParamSet_t, attGyroD), offsetof(DynamicParams_t, attGyroD),0,250},
 
{offsetof(ParamSet_t, rateGyroP), offsetof(DynamicParams_t, rateGyroP),0,250},
{offsetof(ParamSet_t, rateGyroI), offsetof(DynamicParams_t, rateGyroI),0,250},
{offsetof(ParamSet_t, rateGyroD), offsetof(DynamicParams_t, rateGyroD),0,250},
 
{offsetof(ParamSet_t, yawGyroP), offsetof(DynamicParams_t, yawGyroP),0,250},
{offsetof(ParamSet_t, yawGyroI), offsetof(DynamicParams_t, yawGyroI),0,250},
{offsetof(ParamSet_t, yawGyroD), offsetof(DynamicParams_t, yawGyroD),0,250},
 
{offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255},
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
{offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255},
{offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255},
{offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255},
{offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255},
52,6 → 63,19
return result;
}
 
void configuration_fixVariableParams(uint8_t variableNumber) {
// If variable value is to great, limit it.
uint8_t value = variables[variableNumber];
if (value >= 256-VARIABLE_COUNT)
value = 256-VARIABLE_COUNT - 1;
for (uint8_t i=0; i<sizeof(DynamicParams_t); i++) {
uint8_t* pointerToParam =(uint8_t*)(&dynamicParams + i);
if (*pointerToParam == 256-VARIABLE_COUNT + variableNumber) {
*pointerToParam = value;
}
}
}
 
void configuration_applyVariablesToParams(void) {
uint8_t i, src;
uint8_t* pointerToTgt;
119,20 → 143,29
GRN_OFF;
}
 
void configuration_setNormalFlightParameters(void) {
flight_setParameters(staticParams.IFactor, dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP, staticParams.yawIFactor);
void configuration_setNormalFlightMode(void) {
FlightMode_t flight_flightMode;
if (dynamicParams.flightMode < 255/3)
flight_flightMode = FM_RETURN_TO_LEVEL;
else if (dynamicParams.flightMode < 255*2/3)
flight_flightMode = FM_HEADING_HOLD;
else
flight_flightMode = FM_RATE;
flight_setMode(flight_flightMode);
debugOut.analog[20] = flight_flightMode;
}
 
void configuration_setFailsafeFlightParameters(void) {
flight_setParameters(0, 90, 120, 90, 120);
void configuration_setFailsafeFlightMode(void) {
flight_setMode(FM_RETURN_TO_LEVEL);
}
 
// Called after a change in configuration parameters, as a hook for modules to take over changes.
void configuration_paramSetDidChange(void) {
// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.
configuration_setNormalFlightParameters();
// No longer necessary.
// configuration_setNormalFlightMode();
flight_setParameters();
output_setParameters();
// Immediately load changes to output, and also signal the paramset change.
output_init();
}
140,7 → 173,9
void setOtherDefaults(void) {
// Height Control
staticParams.airpressureFilterConstant = 8;
//staticParams.airpressureWindowLength = 0;
staticParams.airpressureWindowLength = 8;
staticParams.airpressureDWindowLength = 24;
 
staticParams.airpressureAccZCorrection = 128+56;
staticParams.heightP = 10;
staticParams.heightD = 30;
148,40 → 183,45
staticParams.heightControlMaxThrottleChange = 10;
// Control
staticParams.flightMode = 248;
staticParams.stickP = 8;
staticParams.stickD = 12;
staticParams.stickYawP = 12;
staticParams.stickThrottleD = 12;
staticParams.stickD = 0;
staticParams.stickYawP = 8;
staticParams.stickThrottleD = 4;
staticParams.minThrottle = 8;
staticParams.maxThrottle = 230;
 
staticParams.externalControl = 0;
staticParams.attitudeControl = 0;
staticParams.motorSmoothing = 0;
staticParams.dynamicStability = 80; // 0 means: Regulation only by decreasing throttle. 200: Only by increasing.
staticParams.gyroP = 60;
staticParams.gyroI = 80;
staticParams.gyroD = 4;
staticParams.attGyroP = PID_NORMAL_VALUE;
staticParams.attGyroI = PID_NORMAL_VALUE;
staticParams.attGyroIMax = 0;
staticParams.attGyroD = PID_NORMAL_VALUE * 3/2;
 
staticParams.rateGyroP = PID_NORMAL_VALUE;
staticParams.rateGyroI = PID_NORMAL_VALUE;
staticParams.rateGyroIMax = 0;
staticParams.rateGyroD = PID_NORMAL_VALUE;
 
staticParams.yawGyroP = PID_NORMAL_VALUE;
staticParams.yawGyroI = PID_NORMAL_VALUE;
staticParams.yawGyroD = PID_NORMAL_VALUE;
// set by gyro-specific code: gyro_setDefaults().
// staticParams.zerothOrderCorrection =
// staticParams.driftCompDivider =
// staticParams.driftCompLimit =
staticParams.dynamicStability = 50;
staticParams.IFactor = 52;
staticParams.yawIFactor = 100;
staticParams.compassMode = 0;
staticParams.compassYawCorrection = 64;
staticParams.compassP = 50;
staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128;
staticParams.compassP = PID_NORMAL_VALUE;
staticParams.levelCorrection[0] = 249; // var1
staticParams.levelCorrection[1] = 250; // var2
 
// Servos
staticParams.servoCount = 7;
staticParams.servoCount = 8;
staticParams.servoManualMaxSpeed = 10;
for (uint8_t i=0; i<2; i++) {
staticParams.servoConfigurations[i].manualControl = 128;
staticParams.servoConfigurations[i].stabilizationFactor = 0;
staticParams.servoConfigurations[i].minValue = 32;
staticParams.servoConfigurations[i].maxValue = 224;
staticParams.servoConfigurations[i].stabilizationFactor = 100;
staticParams.servoConfigurations[i].flags = 0;
}
196,12 → 236,10
staticParams.outputFlash[1].bitmask = 3; //0b11110011;
staticParams.outputFlash[1].timing = 15;
 
staticParams.outputDebugMask = DEBUG_ACC0THORDER;
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
staticParams.outputDebugMask = DEBUG_INVERTED;
staticParams.outputFlags = /* OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | */ OUTPUTFLAGS_USE_ONBOARD_LEDS;
 
staticParams.naviMode = 0; // free.
staticParams.airpressureWindowLength = 0;
staticParams.airpressureDWindowLength = 24;
 
staticParams.heightControlMaxIntegralIn = 125;
staticParams.heightControlMaxIntegralOut = 75;
220,7 → 258,7
staticParams.userParams[i] = i;
}
 
staticParams.bitConfig = CFG_GYRO_SATURATION_PREVENTION;
staticParams.bitConfig = 0;//CFG_GYRO_SATURATION_PREVENTION;
 
memcpy(staticParams.name, "Default\0", 6);
}
230,9 → 268,8
IMUConfig.gyroDWindowLength = 3;
// IMUConfig.gyroDFilterConstant = 1;
IMUConfig.accFilterConstant = 10;
IMUConfig.rateTolerance = 120;
IMUConfig.gyroActivityDamping = 24;
 
gyro_setDefaultParameters();
}
 
239,59 → 276,71
/***************************************************/
/* Default Values for Mixer Table */
/***************************************************/
void motorMixer_default(void) { // Quadro
void outputMixer_default(void) { // Quadro
uint8_t i;
// clear mixer table (but preset throttle)
for (i = 0; i < MAX_MOTORS; i++) {
motorMixer.matrix[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
motorMixer.matrix[i][MIX_PITCH] = 0;
motorMixer.matrix[i][MIX_ROLL] = 0;
motorMixer.matrix[i][MIX_YAW] = 0;
motorMixer.matrix[i][MIX_OPPOSITE_MOTOR] = (uint8_t)-1;
for (i = 0; i < NUM_OUTPUTS; i++) {
outputMixer[i].flightControls[MIXER_SOURCE_THROTTLE] = i < 4 ? (1<<LOG_MOTOR_MIXER_UNIT) : 0;
outputMixer[i].flightControls[MIXER_SOURCE_PITCH] = 0;
outputMixer[i].flightControls[MIXER_SOURCE_ROLL] = 0;
outputMixer[i].flightControls[MIXER_SOURCE_YAW] = 0;
outputMixer[i].oppositeMotor = (uint8_t)-1;
outputMixer[i].auxSource = (uint8_t)-1;
outputMixer[i].outputType = i < 4 ? OUTPUT_TYPE_MOTOR : OUTPUT_TYPE_UNDEFINED;
outputMixer[i].minValue = i < 4 ? 1 : 128;
outputMixer[i].maxValue = i < 4 ? 255 : 128;
}
/*
// default = Quadro+
motorMixer.matrix[0][MIX_PITCH] = +64;
motorMixer.matrix[0][MIX_YAW] = +64;
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1;
outputMixer[0].flightControls[MIXER_SOURCE_PITCH] = -(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[0].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[0].oppositeMotor = 1;
 
motorMixer.matrix[1][MIX_PITCH] = -64;
motorMixer.matrix[1][MIX_YAW] = +64;
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0;
outputMixer[1].flightControls[MIXER_SOURCE_PITCH] = +(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[1].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[1].oppositeMotor = 0;
outputMixer[2].flightControls[MIXER_SOURCE_ROLL] = +(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[2].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[2].oppositeMotor = 3;
 
motorMixer.matrix[2][MIX_ROLL] = -64;
motorMixer.matrix[2][MIX_YAW] = -64;
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3;
outputMixer[3].flightControls[MIXER_SOURCE_ROLL] = -(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[3].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT);
outputMixer[3].oppositeMotor = 2;
 
motorMixer.matrix[3][MIX_ROLL] = +64;
motorMixer.matrix[3][MIX_YAW] = -64;
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2;
outputMixer[8].outputType = OUTPUT_TYPE_SERVO;
outputMixer[8].auxSource = MIXER_SOURCE_AUX_RCCHANNEL;
outputMixer[8].minValue = 40;
outputMixer[8].maxValue = 256-40;
 
memcpy(motorMixer.name, "Quadro +\0", 9);
*/
 
// default = Quadro
motorMixer.matrix[0][MIX_PITCH] = +64;
motorMixer.matrix[0][MIX_ROLL] = +64;
motorMixer.matrix[0][MIX_YAW] = +64;
outputMixer[9].outputType = OUTPUT_TYPE_SERVO;
outputMixer[9].auxSource = MIXER_SOURCE_AUX_RCCHANNEL+1;
outputMixer[9].minValue = 10;
outputMixer[9].maxValue = 256-10;
/*
// default = Quadro X
motorMixer.matrix[0][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[0][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[0][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1;
 
motorMixer.matrix[1][MIX_PITCH] = -64;
motorMixer.matrix[1][MIX_ROLL] = -64;
motorMixer.matrix[1][MIX_YAW] = +64;
motorMixer.matrix[1][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[1][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[1][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0;
 
motorMixer.matrix[2][MIX_PITCH] = +64;
motorMixer.matrix[2][MIX_ROLL] = -64;
motorMixer.matrix[2][MIX_YAW] = -64;
motorMixer.matrix[2][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[2][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[2][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3;
 
motorMixer.matrix[3][MIX_PITCH] = -64;
motorMixer.matrix[3][MIX_ROLL] = +64;
motorMixer.matrix[3][MIX_YAW] = -64;
motorMixer.matrix[3][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[3][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[3][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER);
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2;
 
memcpy(motorMixer.name, "Quadro X\0", 9);
*/
}
 
/***************************************************/
303,8 → 352,8
channelMap.channels[CH_ROLL] = 0;
channelMap.channels[CH_THROTTLE] = 2;
channelMap.channels[CH_YAW] = 3;
channelMap.channels[CH_POTS + 0] = 4;
channelMap.channels[CH_POTS + 1] = 5;
channelMap.channels[CH_POTS + 2] = 6;
channelMap.channels[CH_POTS + 3] = 7;
channelMap.channels[CH_VARIABLES + 0] = 4;
channelMap.channels[CH_VARIABLES + 1] = 5;
channelMap.channels[CH_VARIABLES + 2] = 6;
channelMap.channels[CH_VARIABLES + 3] = 7;
}
/branches/dongfang_FC_rewrite/configuration.h
4,16 → 4,17
#include <inttypes.h>
#include <avr/io.h>
 
#define MAX_CHANNELS 10
#define MAX_MOTORS 12
#define MAX_CONTROLCHANNELS 8
#define MAX_I2CCHANNELS 8
#define MAX_PWMCHANNELS 8
 
// bitmask for VersionInfo_t.HardwareError[0]
#define FC_ERROR0_GYRO_PITCH 0x01
#define FC_ERROR0_GYRO_ROLL 0x02
#define FC_ERROR0_GYRO_YAW 0x04
#define FC_ERROR0_ACC_X 0x08
#define FC_ERROR0_ACC_Y 0x10
#define FC_ERROR0_ACC_Z 0x20
#define FC_ERROR0_GYRO_X 0x01
#define FC_ERROR0_GYRO_Y 0x02
#define FC_ERROR0_GYRO_Z 0x04
#define FC_ERROR0_ACCEL_X 0x08
#define FC_ERROR0_ACCEL_Y 0x10
#define FC_ERROR0_ACCEL_Z 0x20
#define FC_ERROR0_PRESSURE 0x40
#define FC_ERROR1_RES0 0x80
// bitmask for VersionInfo_t.HardwareError[1]
26,12 → 27,14
#define FC_ERROR1_RES2 0x40
#define FC_ERROR1_RES3 0x80
 
#define PID_NORMAL_VALUE 100
 
typedef struct {
uint8_t SWMajor;
uint8_t SWMinor;
uint8_t SWPatch;
uint8_t protoMajor;
uint8_t protoMinor;
uint8_t SWPatch;
uint8_t hardwareErrors[5];
}__attribute__((packed)) VersionInfo_t;
 
38,26 → 41,34
extern VersionInfo_t versionInfo;
 
typedef struct {
/*PMM*/uint8_t gyroP;
/* P */uint8_t gyroI;
/* P */uint8_t gyroD;
/* P */uint8_t compassControlHeading;
uint8_t flightMode;
uint8_t attGyroP; // Attitude mode Proportional (attitude error to control)
uint8_t attGyroI; // Attitude mode Integral (attitude error integral to control. Serves to eliminate permanent attitude errors)
uint8_t attGyroD; // Attitude mode rate
uint8_t rateGyroP; // Rate mode Proportional
uint8_t rateGyroI; // Rate mode Integral (serves to eliminate slow rotation errors in rate mode)
uint8_t rateGyroD; // Rate mode Differential (GyroD)
 
uint8_t yawGyroP;
uint8_t yawGyroI;
uint8_t yawGyroD;
// Control
/* P */uint8_t externalControl;
/* P */uint8_t dynamicStability;
 
uint8_t externalControl;
uint8_t attitudeControl;
// Height control
/*PMM*/uint8_t heightP;
/* P */uint8_t heightI;
/*PMM*/uint8_t heightD;
/* P */uint8_t heightSetting;
uint8_t heightP;
uint8_t heightI;
uint8_t heightD;
uint8_t heightSetting;
 
uint8_t attitudeControl;
 
// Output and servo
/*PMM*/uint8_t output0Timing;
/*PMM*/uint8_t output1Timing;
uint8_t output0Timing;
uint8_t output1Timing;
 
uint8_t servoManualControl[2];
 
87,18 → 98,59
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky.
uint8_t HWTrim;
uint8_t variableOffset;
uint8_t channels[MAX_CHANNELS];
uint8_t channels[MAX_CONTROLCHANNELS];
} ChannelMap_t;
 
extern ChannelMap_t channelMap;
 
#define LOG_MOTOR_MIXER_UNIT 6
#define LOG_DYNAMIC_STABILITY_SCALER 6
 
typedef enum {
MIXER_SOURCE_ROLL = 0,
MIXER_SOURCE_PITCH = 1,
MIXER_SOURCE_THROTTLE = 2,
MIXER_SOURCE_YAW = 3
} MixerSource_Control;
 
typedef enum {
MIXER_SOURCE_AUX_GIMBAL_ROLL = 0,
MIXER_SOURCE_AUX_GIMBAL_PITCH = 1,
MIXER_SOURCE_AUX_RCCHANNEL = 2,
MIXER_SOURCE_AUX_END = 2 + MAX_CONTROLCHANNELS
} MixerSourceAux;
 
typedef enum {
I2C0 = 0,
I2C1 = 1,
I2C2 = 2,
I2C3 = 3,
I2C4 = 4,
I2C5 = 5,
I2C6 = 6,
I2C7 = 7,
SERVO0 = MAX_I2CCHANNELS,
NUM_OUTPUTS = 8 + MAX_PWMCHANNELS
} MixerDestination;
 
typedef struct {
char name[12];
int8_t matrix[MAX_MOTORS][5];
//int8_t oppositeMotor[MAX_MOTORS];
}__attribute__((packed)) MotorMixer_t;
extern MotorMixer_t motorMixer;
uint8_t outputType;
uint8_t minValue;
uint8_t maxValue;
int8_t flightControls[4];
uint8_t auxSource; // for selecting a gimbal axis or an RC channel.
uint8_t oppositeMotor; // only relevant where used to control a multicopter motor.
}__attribute__((packed)) MixerMatrixRow_t;
 
typedef MixerMatrixRow_t OutputMixer_t[NUM_OUTPUTS];
extern OutputMixer_t outputMixer;
 
typedef enum {
OUTPUT_TYPE_UNDEFINED,
OUTPUT_TYPE_MOTOR,
OUTPUT_TYPE_SERVO
} outputTypes;
 
typedef struct {
int16_t offsets[3];
} sensorOffset_t;
106,8 → 158,6
typedef struct {
uint8_t manualControl;
uint8_t stabilizationFactor;
uint8_t minValue;
uint8_t maxValue;
uint8_t flags;
} Servo_t;
 
119,21 → 169,24
} output_flash_t;
 
typedef struct {
/* Set up so that
* Nose-up is positive on pitch
* Roll-right is positive on roll
* Turn-ccw is positive on yaw
*/
uint8_t gyroQuadrant;
/*
* Set up so that:
* Acc. Z is +1g in normal attitude
* Acc. X gets positive in a left roll
* Acc. Y gets positive in a nose-up attitude.
*/
uint8_t accQuadrant;
uint8_t imuReversedFlags;
 
uint8_t gyroPIDFilterConstant;
uint8_t gyroDWindowLength;
// uint8_t gyroDFilterConstant;
uint8_t accFilterConstant;
 
uint8_t zerothOrderCorrection;
uint8_t rateTolerance;
 
uint8_t gyroActivityDamping;
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung)
uint8_t driftCompLimit; // limit for gyrodrift compensation
} IMUConfig_t;
 
extern IMUConfig_t IMUConfig;
143,6 → 196,8
// Global bitflags
uint8_t bitConfig; // see upper defines for bitcoding
 
uint8_t flightMode;
// uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
// uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
// uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
150,11 → 205,23
uint8_t levelCorrection[2];
 
// Control
uint8_t gyroP;
uint8_t gyroI;
uint8_t gyroD;
uint8_t attGyroP; // Attitude mode Proportional (attitude error to control)
uint8_t attGyroI; // Attitude mode Integral (attitude error integral to control. Serves to eliminate permanent attitude errors)
uint8_t attGyroIMax; // Attitude mode Integral max. limit
uint8_t attGyroD; // Attitude mode rate
uint8_t rateGyroP; // Rate mode Proportional
uint8_t rateGyroI; // Rate mode Integral (serves to eliminate slow rotation errors in rate mode)
uint8_t rateGyroIMax; // Rate mode Integral max. limit
uint8_t rateGyroD; // Rate mode Differential (GyroD)
 
uint8_t yawGyroP;
uint8_t yawGyroI;
uint8_t yawGyroD;
uint8_t externalControl; // for serial Control
uint8_t attitudeControl;
uint8_t dynamicStability;
 
uint8_t stickP;
uint8_t stickD;
161,16 → 228,11
uint8_t stickYawP;
uint8_t stickThrottleD;
 
// Idle throttle. Affects the throttle stick only.
uint8_t minThrottle;
// Value of max. throttle stick.
uint8_t maxThrottle;
 
uint8_t externalControl; // for serial Control
uint8_t motorSmoothing;
uint8_t dynamicStability; // PID limit for Attitude controller
 
uint8_t IFactor;
uint8_t yawIFactor;
 
uint8_t compassMode; // bitflag thing.
uint8_t compassYawCorrection;
uint8_t compassBendingReturnSpeed;
230,33 → 292,31
extern ParamSet_t staticParams;
 
// MKFlags
#define MKFLAG_MOTOR_RUN (1<<0)
#define MKFLAG_MOTOR_RUN (1<<0)
//#define MKFLAG_FLY (1<<1)
#define MKFLAG_CALIBRATE (1<<2)
#define MKFLAG_START (1<<3)
//#define MKFLAG_CALIBRATE (1<<2)
#define MKFLAG_EMERGENCY_FLIGHT (1<<4)
#define MKFLAG_LOWBAT (1<<5)
#define MKFLAG_RESERVE2 (1<<6)
#define MKFLAG_RESERVE3 (1<<7)
#define MKFLAG_LOWBAT (1<<5)
//#define MKFLAG_RESERVE2 (1<<6)
//#define MKFLAG_RESERVE3 (1<<7)
 
// bit mask for staticParams.bitConfig
#define CFG_SIMPLE_HEIGHT_CONTROL (1<<0)
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1)
#define CFG_HEADING_HOLD (1<<2)
#define CFG_UNUSED (1<<2)
#define CFG_COMPASS_ENABLED (1<<3)
#define CFG_UNUSED (1<<4)
#define CFG_UNUSED2 (1<<4)
#define CFG_NAVI_ENABLED (1<<5)
#define CFG_AXIS_COUPLING_ENABLED (1<<6)
#define CFG_UNUSED3 (1<<6)
#define CFG_GYRO_SATURATION_PREVENTION (1<<7)
 
#define IMU_REVERSE_GYRO_PR (1<<0)
#define IMU_REVERSE_GYRO_YAW (1<<1)
#define IMU_REVERSE_ACC_XY (1<<2)
#define IMU_REVERSE_ACC_Z (1<<3)
#define IMU_REVERSE_GYRO_XY (1<<0)
#define IMU_REVERSE_GYRO_Z (1<<1)
#define IMU_REVERSE_ACCEL_XY (1<<2)
#define IMU_REVERSE_ACCEL_Z (1<<3)
 
#define ATMEGA644 0
#define ATMEGA644P 1
#define SYSCLK F_CPU
 
// Not really a part of configuration, but LEDs and HW s test are the same.
#define RED_OFF {if((boardRelease == 10) || (boardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
267,16 → 327,10
#define GRN_FLASH PORTB ^= (1<<PORTB1)
 
// Mixer table
#define MIX_PITCH 0
#define MIX_ROLL 1
#define MIX_THROTTLE 2
#define MIX_YAW 3
#define MIX_OPPOSITE_MOTOR 4
 
#define VARIABLE_COUNT 8
 
extern volatile uint8_t MKFlags;
extern uint8_t requiredMotors;
extern int16_t variables[VARIABLE_COUNT]; // The "Poti"s.
extern uint8_t boardRelease;
extern uint8_t CPUType;
287,10 → 341,10
void IMUConfig_default(void);
void channelMap_default(void);
void paramSet_default(uint8_t setnumber);
void motorMixer_default(void);
void outputMixer_default(void);
 
void configuration_setNormalFlightParameters(void);
void configuration_setFailsafeFlightParameters(void);
void configuration_setNormalFlightMode(void);
void configuration_setFailsafeFlightMode(void);
void configuration_applyVariablesToParams(void);
 
void setCPUType(void);
/branches/dongfang_FC_rewrite/controlMixer.c
2,26 → 2,26
#include "controlMixer.h"
#include "rc.h"
#include "heightControl.h"
#include "attitudeControl.h"
#include "externalControl.h"
#include "compassControl.h"
#include "failsafeControl.h"
#include "naviControl.h"
#include "configuration.h"
#include "attitude.h"
//#include "attitude.h"
#include "commands.h"
#include "output.h"
#include "debug.h"
#include "definitions.h"
 
// uint16_t maxControl[2] = { 0, 0 };
// uint16_t controlActivity = 0;
int16_t controls[4] = { 0, 0, 0, 0 };
int16_t controls[4];
 
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand = COMMAND_NONE;
uint8_t lastCommand;
uint8_t lastArgument;
 
uint8_t isCommandRepeated = 0;
uint8_t controlMixer_didReceiveSignal = 0;
uint8_t isCommandRepeated;
uint8_t controlMixer_didReceiveSignal;
 
/*
* This could be expanded to take arguments from ohter sources than the RC
43,10 → 43,23
return isCommandRepeated;
}
 
/*
* TODO: This assumes R/C as source. Not necessarily true.
*/
void controlMixer_updateVariables(void) {
uint8_t i;
for (i=0; i < VARIABLE_COUNT; i++) {
int16_t targetvalue = RC_getVariable(i);
if (targetvalue < 0)
variables[i] = 0;
else if (targetvalue > 255)
variables[i] = 255;
else variables[i] = targetvalue;
}
}
 
void controlMixer_setNeutral() {
for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
variables[i] = RC_getVariable(i);
}
controlMixer_updateVariables();
EC_setNeutral();
HC_setGround();
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.
55,24 → 68,6
// MKFlags |= MKFLAG_CALIBRATE;
}
 
/*
* Update potentiometer values with limited slew rate. Could be made faster if desired.
* TODO: It assumes R/C as source. Not necessarily true.
*/
void controlMixer_updateVariables(void) {
uint8_t i;
int16_t targetvalue;
for (i=0; i < VARIABLE_COUNT; i++) {
targetvalue = RC_getVariable(i);
if (targetvalue < 0)
targetvalue = 0;
if (variables[i] < targetvalue && variables[i] < 255)
variables[i]++;
else if (variables[i] > 0 && variables[i] > targetvalue)
variables[i]--;
}
}
 
uint8_t controlMixer_getSignalQuality(void) {
uint8_t rcQ = RC_getSignalQuality();
uint8_t ecQ = EC_getSignalQuality();
117,7 → 112,7
*/
 
void controlMixer_periodicTask(void) {
int16_t tempPRTY[4] = { 0, 0, 0, 0 };
int16_t tempRPTY[4] = { 0, 0, 0, 0 };
 
// Decode commands.
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
143,6 → 138,8
isCommandRepeated = (lastCommand == ecCommand);
lastCommand = ecCommand;
lastArgument = EC_getArgument();
if (ecCommand == COMMAND_GYROCAL) debugOut.digital[0] |= DEBUG_COMMAND;
if (ecCommand == COMMAND_ACCCAL) debugOut.digital[1] |= DEBUG_COMMAND;
} else {
// Both sources have no command, or one or both are out.
// Just set to false. There is no reason to check if the none-command was repeated anyway.
151,36 → 148,38
}
// This will init the values (not just add to them).
RC_periodicTaskAndPRTY(tempPRTY);
RC_periodicTaskAndRPTY(tempRPTY);
 
// Add external control to RC
EC_periodicTaskAndPRTY(tempPRTY);
EC_periodicTaskAndRPTY(tempRPTY);
 
#ifdef USE_DIRECT_GPS
if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
navigation_periodicTaskAndPRTY(tempPRTY);
navigation_periodicTaskAndRPTY(tempRPTY);
#endif
 
// Add compass control (could also have been before navi, they are independent)
CC_periodicTaskAndPRTY(tempPRTY);
// CC_periodicTaskAndRPTY(tempRPTY);
 
FC_periodicTaskAndPRTY(tempPRTY);
 
FC_periodicTaskAndRPTY(tempRPTY);
// This is temporary. There might be some emergency height control also.
if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
// Add height control (could also have been before navi and/or compass, they are independent)
HC_periodicTaskAndPRTY(tempPRTY);
HC_periodicTaskAndRPTY(tempRPTY);
 
// Add attitude control (could also have been before navi and/or compass, they are independent)
AC_getPRTY(tempPRTY);
// AC_getRPTY(tempRPTY);
}
 
// Commit results to global variable and also measure control activity.
controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
controls[CONTROL_PITCH] = tempPRTY[CONTROL_PITCH];
controls[CONTROL_ROLL] = tempPRTY[CONTROL_ROLL];
controls[CONTROL_YAW] = tempPRTY[CONTROL_YAW];
// dampenControlActivity();
controls[CONTROL_ROLL] = tempRPTY[CONTROL_ROLL];
controls[CONTROL_PITCH] = tempRPTY[CONTROL_PITCH];
controls[CONTROL_THROTTLE] = tempRPTY[CONTROL_THROTTLE];
controls[CONTROL_YAW] = tempRPTY[CONTROL_YAW];
 
debugOut.analog[14] =controls[CONTROL_ROLL];
debugOut.analog[15] =controls[CONTROL_PITCH];
// We can safely do this even with a bad signal - the variables will not have been updated then.
configuration_applyVariablesToParams();
/branches/dongfang_FC_rewrite/controlMixer.h
1,3 → 1,6
#ifndef _CONTROLMIXER_H
#define _CONTROLMIXER_H
 
#include <inttypes.h>
/*
* An attempt to define a generic control. That could be an R/C receiver, an external control
9,66 → 12,6
*/
 
/*
* Signal qualities, used to determine the availability of a control.
* NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost.
* SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage
* or disengage emergency landing.
* SIGNAL_OK means the signal is usable for flight.
* SIGNAL_GOOD means the signal can also be used for setting parameters.
*/
#define NO_SIGNAL 0
#define SIGNAL_LOST 1
#define SIGNAL_BAD 2
#define SIGNAL_OK 3
#define SIGNAL_GOOD 4
 
/*
* The PRTY arrays
*/
#define CONTROL_PITCH 0
#define CONTROL_ROLL 1
#define CONTROL_THROTTLE 2
#define CONTROL_YAW 3
 
/*
* Looping flags.
* LOOPING_UP || LOOPING_DOWN <=> LOOPING_PITCH_AXIS
* LOOPING_LEFT || LOOPING_RIGHT <=> LOOPING_ROLL_AXIS
*/
#define LOOPING_UP (1<<0)
#define LOOPING_DOWN (1<<1)
#define LOOPING_LEFT (1<<2)
#define LOOPING_RIGHT (1<<3)
#define LOOPING_PITCH_AXIS (1<<4)
#define LOOPING_ROLL_AXIS (1<<5)
 
/*
* This is only relevant for "abstract controls" ie. all control sources have the
* same interface. This struct of code pointers is used like an abstract class
* definition from object-oriented languages, and all control input implementations
* will declare an instance of the stuct (=implementation of the abstract class).
*/
typedef struct {
/* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getPitch)(void);
 
/* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getRoll)(void);
 
/* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getYaw)(void);
 
/* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */
uint16_t(*getThrottle)(void);
 
/* Signal quality, by the above SIGNAL_... definitions. */
uint8_t (*getSignalQuality)(void);
 
/* Calibrate sticks to their center positions (only relevant for R/C, really) */
void (*calibrate)(void);
} t_control;
 
/*
* Our output.
*/
extern int16_t controls[4];
95,16 → 38,7
uint8_t controlMixer_getSignalQuality(void);
extern uint8_t controlMixer_didReceiveSignal;
 
/*
* The controls operate in [-1024, 1024] just about.
* Throttle is [0..255] just about.
*/
// Scale controls to 1 byte:
#define CONTROL_SCALING (1024/256)
 
// Scale throttle levels to byte:
#define MOTOR_SCALING (1024/256)
 
/*
* Gets the argument for the current command (a number).
*
122,3 → 56,5
*/
uint8_t controlMixer_getArgument(void);
uint8_t controlMixer_isCommandRepeated(void);
 
#endif
/branches/dongfang_FC_rewrite/debug.h
0,0 → 1,69
#ifndef _DEBUG_H
#define _DEBUG_H
 
/*
* Some digital output useful for timing and tracing things with a scope.
*/
#define J3HIGH PORTD |= (1<<PORTD5)
#define J3LOW PORTD &= ~(1<<PORTD5)
#define J3TOGGLE PORTD ^= (1<<PORTD5)
 
#define J4HIGH PORTD |= (1<<PORTD4)
#define J4LOW PORTD &= ~(1<<PORTD4)
#define J4TOGGLE PORTD ^= (1<<PORTD4)
 
#define J5HIGH PORTD |= (1<<PORTD3)
#define J5LOW PORTD &= ~(1<<PORTD3)
#define J5TOGGLE PORTD ^= (1<<PORTD3)
 
// invert means: An "1" bit in digital debug data make a LOW on the output.
#define DIGITAL_DEBUG_INVERT 0
 
/*
* Some digital debugs. A digital debug is 2 signals on the 2 LED outputs,
* turned on and off depending on some condtions given in the code.
* Only one can be selected, by defining DIGITAL_DEBUG_MASK to the value
* of the debug.
* In the code one can do like:
* if (whatever_condition) {
* DebugOut.Digital[0] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[0] &= ~DEBUG_MYOWNDEBUGGER;
* }
* ...
* if (whatever_other_condition) {
* DebugOut.Digital[1] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[1] &= ~DEBUG_MYOWNDEBUGGER;
* }
*
* Digital debugs may be added as desired, and removed when the mystery
* at hand is resolved.
*/
 
#define DEBUG_MAINLOOP_TIMER 1
#define DEBUG_HEIGHT_DIFF 2
#define DEBUG_INVERTED 4
#define DEBUG_COMMAND 8
#define DEBUG_COMPASS 16
#define DEBUG_PRESSURERANGE 32
#define DEBUG_CLIP 64
#define DEBUG_SENSORLIMIT 128
 
#define OUTPUTFLAGS_INVERT_0 1 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option)
#define OUTPUTFLAGS_INVERT_1 2 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option)
#define OUTPUTFLAGS_FLASH_0_AT_BEEP 4 // Flash LED when beeper beeps
#define OUTPUTFLAGS_FLASH_1_AT_BEEP 8 // Flash LED when beeper beeps
#define OUTPUTFLAGS_USE_ONBOARD_LEDS 16 // Control on-board LEDs in addition to outputs
#define OUTPUTFLAGS_TEST_OFF 32 // For testing: Turn off both outputs
#define OUTPUTFLAGS_TEST_ON 64 // For testing: Turn on both outputs
 
// For practical reasons put here instead of in uart0.h
typedef struct {
uint8_t digital[2];
int16_t analog[32]; // debug values
}__attribute__((packed)) DebugOut_t;
 
extern DebugOut_t debugOut;
 
#endif
/branches/dongfang_FC_rewrite/definitions.h
0,0 → 1,45
#ifndef _DEFINITIONS_H
#define _DEFINITIONS_H
 
/*
* Signal qualities, used to determine the availability of a control.
* NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost.
* SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage
* or disengage emergency landing.
* SIGNAL_OK means the signal is usable for flight.
* SIGNAL_GOOD means the signal can also be used for setting parameters.
*/
#define NO_SIGNAL 0
#define SIGNAL_LOST 1
#define SIGNAL_BAD 2
#define SIGNAL_OK 3
#define SIGNAL_GOOD 4
 
/*
* The RPTY arrays
*/
#define CONTROL_ROLL 0
#define CONTROL_PITCH 1
#define CONTROL_THROTTLE 2
#define CONTROL_YAW 3
 
 
/*
* The controls operate in [-1024, 1024] just about.
* We compute control input and output in terms of 11 bit ranges then.
*/
 
#define LOG_CONTROL_RANGE 11
#define CONTROL_RANGE (1<<LOG_CONTROL_RANGE)
 
// For scaling control input quantities to bytes (configuration parameters).
#define LOG_CONTROL_BYTE_SCALING (LOG_CONTROL_RANGE - 8)
 
// MK BLC 1.x use 8 bit values. MK BLC 2.x use 10 bits. 10 bits is not implemented and probably doesnt
// make a practical difference anyway.
#define LOG_I2C_CONTROL_RANGE 8
 
// Scale throttle levels to MK BLCs 8 bit range:
#define LOG_I2C_CONTROL_SCALING (LOG_CONTROL_RANGE - LOG_I2C_CONTROL_RANGE)
 
#endif
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
9,7 → 9,6
#include "output.h"
#include "isqrt.h"
#include "attitude.h"
#include "dongfangMath.h"
#include "attitude.h"
 
typedef enum {
17,7 → 16,7
NAVI_FLIGHT_MODE_FREE,
NAVI_FLIGHT_MODE_AID,
NAVI_FLIGHT_MODE_HOME,
} FlightMode_t;
} NaviMode_t;
 
/*
* This is not read from internally. It only serves to let a pilot/debugger
53,7 → 52,7
// GPS coordinates for home position
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
// the current flight mode
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
NaviMode_t naviMode = NAVI_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
int8_t naviStatus;
60,22 → 59,22
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
static NaviMode_t naviModeOld = NAVI_FLIGHT_MODE_UNDEF;
 
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = NAVI_FLIGHT_MODE_FREE;
naviMode = NAVI_FLIGHT_MODE_FREE;
} else {
if (dynamicParams.naviMode < 50)
flightMode = NAVI_FLIGHT_MODE_FREE;
naviMode = NAVI_FLIGHT_MODE_FREE;
else if (dynamicParams.naviMode < 180)
flightMode = NAVI_FLIGHT_MODE_AID;
naviMode = NAVI_FLIGHT_MODE_AID;
else
flightMode = NAVI_FLIGHT_MODE_HOME;
naviMode = NAVI_FLIGHT_MODE_HOME;
}
 
if (flightMode != flightModeOld) {
if (naviMode != naviModeOld) {
beep(100);
flightModeOld = flightMode;
naviModeOld = naviMode;
}
}
 
107,11 → 106,9
}
 
// checks nick and roll sticks for manual control
uint8_t navi_isManuallyControlled(int16_t* PRTY) {
debugOut.analog[26] = PRTY[CONTROL_PITCH];
debugOut.analog[27] = PRTY[CONTROL_ROLL];
if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
&& abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
uint8_t navi_isManuallyControlled(int16_t* RPTY) {
if (abs(RPTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
&& abs(RPTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
return 0;
else
return 1;
268,7 → 265,7
}
}
 
void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
void navigation_periodicTaskAndRPTY(int16_t* RPTY) {
static uint8_t GPS_P_Delay = 0;
static uint16_t beep_rythm = 0;
static uint16_t navi_testOscPrescaler = 0;
280,7 → 277,7
return;
}
 
navi_updateFlightMode();
navi_updateNaviMode();
 
navi_testOscPrescaler++;
if (navi_testOscPrescaler == 488) {
314,7 → 311,7
case INVALID: // invalid gps data
navi_setNeutral();
naviStatus = NAVI_STATUS_INVALID_GPS;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
if (naviMode != NAVI_FLIGHT_MODE_FREE) {
beep(1); // beep if signal is neccesary
}
break;
334,7 → 331,7
// if the gps data quality is good
beep_rythm++;
if (navi_isGPSSignalOK()) {
switch (flightMode) { // check what's to do
switch (naviMode) { // check what's to do
case NAVI_FLIGHT_MODE_FREE:
// update hold position to current gps position
navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
345,7 → 342,7
 
case NAVI_FLIGHT_MODE_AID:
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user
// update hold point to current gps position
navi_writeCurrPositionTo(&holdPosition);
// disable gps control
377,7 → 374,7
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
navi_writeCurrPositionTo(&holdPosition);
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user
navi_setNeutral();
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
} else {// GPS control active
389,7 → 386,7
beep(50); // signal invalid home position
// try to hold at least the position as a fallback option
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) {
if (navi_isManuallyControlled(RPTY)) {
// MK controlled by user
navi_setNeutral();
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
414,7 → 411,7
// disable gps control
navi_setNeutral();
naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
if (naviMode != NAVI_FLIGHT_MODE_FREE) {
// beep if signal is not sufficient
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
beep(100);
428,14 → 425,6
break;
} // eof GPSInfo.status
 
PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
 
//debugOut.analog[16] = flightMode;
//debugOut.analog[17] = naviStatus;
 
/*
debugOut.analog[18] = naviSticks[CONTROL_PITCH];
debugOut.analog[19] = naviSticks[CONTROL_ROLL];
*/
RPTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
RPTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
}
/branches/dongfang_FC_rewrite/eeprom.c
3,12 → 3,13
#endif
 
#include "eeprom.h"
#include "printf_P.h"
#include "output.h"
//#include "output.h"
#include "configuration.h"
#include "analog.h"
#include <avr/wdt.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <stdio.h>
 
// byte array in eeprom
uint8_t EEPromArray[E2END + 1] EEMEM;
152,26 → 153,17
/***************************************************/
/* MixerTable */
/***************************************************/
void motorMixer_writeToEEProm(void) {
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&motorMixer, EEPROM_ADR_MIXER_TABLE, sizeof(MotorMixer_t));
void outputMixer_writeToEEProm(void) {
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&outputMixer, EEPROM_ADR_MIXER_TABLE, sizeof(OutputMixer_t));
}
 
void motorMixer_readOrDefault(void) {
void outputMixer_readOrDefault(void) {
// load mixer table
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&motorMixer, EEPROM_ADR_MIXER_TABLE, sizeof(MotorMixer_t))) {
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&outputMixer, EEPROM_ADR_MIXER_TABLE, sizeof(OutputMixer_t))) {
printf("\n\rwriting default motor mixer");
motorMixer_default(); // Quadro
motorMixer_writeToEEProm();
outputMixer_default(); // Quadro
outputMixer_writeToEEProm();
}
// determine motornumber
requiredMotors = 0;
for (uint8_t i=0; i<MAX_MOTORS; i++) {
if (motorMixer.matrix[i][MIX_THROTTLE])
requiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)", motorMixer.name, requiredMotors);
printf("\n\r===================================");
}
 
/***************************************************/
209,12 → 201,12
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t));
}
 
uint8_t accOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
uint8_t accelOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accelOffset, EEPROM_ADR_ACCELOFFSET, sizeof(sensorOffset_t));
}
 
void accOffset_writeToEEProm(void) {
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
void accelOffset_writeToEEProm(void) {
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accelOffset, EEPROM_ADR_ACCELOFFSET, sizeof(sensorOffset_t));
}
 
/***************************************************/
/branches/dongfang_FC_rewrite/eeprom.h
3,7 → 3,6
 
#include <inttypes.h>
#include "configuration.h"
#include "analog.h"
 
#define EEPROM_ADR_PARAM_BEGIN 0
#define EEPROM_CHECKSUMMED_BLOCK_OVERHEAD 3
16,23 → 15,23
//#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(ChannelMap_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(MixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
 
#define EEPROM_ADR_ACCOFFSET 16
#define EEPROM_ADR_ACCELOFFSET 16
#define EEPROM_ADR_GYROOFFSET 32
#define EEPROM_ADR_GYROAMPLIFIER 48
#define EEPROM_ADR_CHANNELMAP 64
#define EEPROM_ADR_IMU_CONFIG 100
#define EEPROM_ADR_MIXER_TABLE 128
#define EEPROM_ADR_PARAMSET_BEGIN 256
#define EEPROM_ADR_PARAMSET_BEGIN 384
 
#define CHANNELMAP_REVISION 0
#define EEPARAM_REVISION 3
#define EEMIXER_REVISION 1
#define EEPARAM_REVISION 5
#define EEMIXER_REVISION 3
#define SENSOROFFSET_REVISION 0
#define IMUCONFIG_REVISION 0
 
void paramSet_readOrDefault(void);
void channelMap_readOrDefault(void);
void motorMixer_readOrDefault(void);
void outputMixer_readOrDefault(void);
void IMUConfig_readOrDefault(void);
void IMUConfig_writeToEEprom(void);
 
43,7 → 42,7
void channelMap_writeToEEProm(void);
 
//uint8_t motorMixer_readFromEEProm(void);
void motorMixer_writeToEEProm(void);
void outputMixer_writeToEEProm(void);
 
uint8_t gyroAmplifierOffset_readFromEEProm(void);
void gyroAmplifierOffset_writeToEEProm(void);
51,8 → 50,8
uint8_t gyroOffset_readFromEEProm(void);
void gyroOffset_writeToEEProm(void);
 
uint8_t accOffset_readFromEEProm(void);
void accOffset_writeToEEProm(void);
uint8_t accelOffset_readFromEEProm(void);
void accelOffset_writeToEEProm(void);
 
uint8_t getParamByte(uint16_t param_id);
void setParamByte(uint16_t param_id, uint8_t value);
/branches/dongfang_FC_rewrite/externalControl.c
1,15 → 1,16
#include "externalControl.h"
#include "configuration.h"
#include "controlMixer.h"
//#include "controlMixer.h"
#include "definitions.h"
 
ExternalControl_t externalControl;
uint8_t externalControlActive = 0;
// int16_t EC_PRTY[4];
volatile uint8_t externalControlActive;
// TODO: Who is going to call this
 
void EC_setNeutral(void) {
// if necessary. From main.c.
externalControl.config = 0;
externalControl.command = 0;
externalControl.argument = 0;
externalControl.pitch = 0;
externalControl.roll = 0;
externalControl.yaw = 0;
19,22 → 20,22
externalControl.digital[0] = 0x55;
}
 
void EC_periodicTaskAndPRTY(int16_t* PRTY) {
void EC_periodicTaskAndRPTY(int16_t* RPTY) {
if (externalControlActive) {
externalControlActive--;
PRTY[CONTROL_PITCH] += externalControl.pitch * (int16_t) staticParams.stickP;
PRTY[CONTROL_ROLL] += externalControl.roll * (int16_t) staticParams.stickP;
PRTY[CONTROL_THROTTLE] += externalControl.throttle;
PRTY[CONTROL_YAW] += externalControl.yaw; // No stickP or similar??????
RPTY[CONTROL_PITCH] += externalControl.pitch * (int16_t) staticParams.stickP;
RPTY[CONTROL_ROLL] += externalControl.roll * (int16_t) staticParams.stickP;
RPTY[CONTROL_THROTTLE] += externalControl.throttle;
RPTY[CONTROL_YAW] += externalControl.yaw; // No stickP or similar??????
}
}
 
uint8_t EC_getArgument(void) {
return externalControl.config;
return externalControl.argument;
}
 
uint8_t EC_getCommand(void) {
return externalControl.free;
return externalControl.command;
}
 
// not implemented.
51,7 → 52,7
// Configured and heard from
return SIGNAL_OK;
 
if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128))
if (dynamicParams.externalControl < 128)
// External control is not even configured.
return NO_SIGNAL;
 
/branches/dongfang_FC_rewrite/externalControl.h
7,20 → 7,20
typedef struct {
uint8_t digital[2];
uint8_t remoteButtons;
int8_t pitch;
int8_t roll;
int8_t yaw;
int8_t pitch;
int8_t roll;
int8_t yaw;
uint8_t throttle;
int8_t height;
uint8_t free; // Let's use that for commands now.
int8_t height;
uint8_t command; // Let's use that for commands now.
uint8_t frame;
uint8_t config; // Let's use that for arguemnts.
uint8_t argument; // Let's use that for arguemnts.
}__attribute__((packed)) ExternalControl_t;
 
extern ExternalControl_t externalControl;
extern uint8_t externalControlActive;
extern volatile uint8_t externalControlActive;
 
void EC_periodicTaskAndPRTY(int16_t* PRTY);
void EC_periodicTaskAndRPTY(int16_t* RPTY);
uint8_t EC_getArgument(void);
uint8_t EC_getCommand(void);
int16_t EC_getVariable(uint8_t varNum);
/branches/dongfang_FC_rewrite/failsafeControl.c
1,22 → 1,19
#include "controlMixer.h"
#include "timer0.h"
#include "configuration.h"
#include "twimaster.h"
//#include "twimaster.h"
#include "flight.h"
#include "output.h"
//#include "output.h"
#include "definitions.h"
#include "beeper.h"
 
uint16_t emergencyFlightTime;
uint32_t emergencyFlightEndTimeMillis;
 
void FC_setNeutral(void) {
configuration_setNormalFlightParameters();
configuration_setNormalFlightMode();
}
 
void FC_periodicTaskAndPRTY(uint16_t* PRTY) {
 
debugOut.analog[25] = emergencyFlightTime;
 
// debugOut.analog[30] = controlMixer_getSignalQuality();
 
void FC_periodicTaskAndRPTY(int16_t* RPTY) {
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
if (controlMixer_didReceiveSignal)
beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
25,17 → 22,12
if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
if (isFlying > 256) {
MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing
configuration_setFailsafeFlightParameters();
configuration_setFailsafeFlightMode();
// Set the time in whole seconds.
if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP)
emergencyFlightTime = 0xffff;
else
emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP;
emergencyFlightEndTimeMillis = millisClock + (staticParams.emergencyFlightDuration * 1000L);
}
} else {
if (emergencyFlightTime) {
emergencyFlightTime--;
} else {
if (millisClock > emergencyFlightEndTimeMillis) {
// stop motors but stay in emergency flight.
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
42,22 → 34,27
}
 
// In either case, use e. throttle and neutral controls. TODO: If there is supposed to be a navi come-home, this should affect RC control only and not navi.
PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle
PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0;
RPTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle << 3; // Set emergency throttle
RPTY[CONTROL_PITCH] = RPTY[CONTROL_ROLL] = RPTY[CONTROL_YAW] = 0;
} else {
// Signal is OK.
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
configuration_setNormalFlightParameters();
// configuration_setNormalFlightParameters();
}
// A hack (permenent?): Keep re-setting the parameters, in order to support dynamic params.
configuration_setNormalFlightMode();
}
 
/*
* If a Bl-Ctrl is missing, prevent takeoff.
* Feature unnecessary, removed.
*/
if (missingMotor) {
/*
if ((MKFlags & MKFLAG_MOTOR_RUN) && missingMotor) {
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition
PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff
if (isFlying > 1 && isFlying < 50 && RPTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition
RPTY[CONTROL_THROTTLE] = staticParams.minThrottle << 3; // reduce throttle to min to prevent takeoff
}
*/
}
/branches/dongfang_FC_rewrite/failsafeControl.h
1,3 → 1,3
#include <inttypes.h>
void FC_setNeutral(void);
void FC_periodicTaskAndPRTY(int16_t* PRTY);
void FC_periodicTaskAndRPTY(int16_t* RPTY);
/branches/dongfang_FC_rewrite/flight.c
1,16 → 1,20
#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include <math.h>
//#include "eeprom.h"
#include "flight.h"
#include "output.h"
#include "uart0.h"
//#include "uart0.h"
 
// Necessary for external control and motor test
#include "twimaster.h"
//#include "twimaster.h"
#include "attitude.h"
#include "analog.h"
#include "controlMixer.h"
#include "commands.h"
#include "heightControl.h"
#include "definitions.h"
#include "debug.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
17,65 → 21,78
#include "compassControl.h"
#endif
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
* These are no longer maintained, just left at 0. The original implementation just summed the acc.
* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
*/
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
uint8_t ki;
int16_t targetHeading;
int32_t IPart[2];
FlightMode_t flight_flightMode = FM_UNINITALIZED;
int16_t minThrottle, maxThrottle;
 
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
switch (staticParams.motorSmoothing) {
case 0:
return newvalue;
case 1:
return (oldvalue + newvalue) / 2;
case 2:
if (newvalue > oldvalue)
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
else
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
case 3:
if (newvalue < oldvalue)
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
else
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
default:
return newvalue;
/*
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
switch (staticParams.motorSmoothing) {
case 0:
return newvalue;
case 1:
return (oldvalue + newvalue) / 2;
case 2:
if (newvalue > oldvalue)
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
else
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
case 3:
if (newvalue < oldvalue)
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
else
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
default:
return newvalue;
}
}
*/
 
void resetIParts(void) {
IPart[X] = IPart[Y] = 0;
}
 
void flight_setMode(FlightMode_t _flightMode) {
if (flight_flightMode != _flightMode) {
resetIParts();
flight_flightMode = _flightMode;
}
}
 
void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
ki = _ki;
gyroPFactor = _gyroPFactor;
gyroIFactor = _gyroIFactor;
yawPFactor = _yawPFactor;
yawIFactor = _yawIFactor;
// To be called only when necessary.
void flight_setParameters(void) {
minThrottle = staticParams.minThrottle << LOG_CONTROL_BYTE_SCALING;
maxThrottle = staticParams.maxThrottle << LOG_CONTROL_BYTE_SCALING;
}
 
void flight_setGround() {
// Just reset all I terms.
IPart[PITCH] = IPart[ROLL] = 0;
headingError = 0;
// A heuristic when the yaw attitude cannot be used for yaw control because the airframe is near-vertical or inverted.
// Fum of abs(pitch) and abs(roll) greater than 75 degrees or about 1.3 radians.
uint8_t isHeadingUnaccountable(void) {
int16_t x = attitude[X];
if (x<0) x = -x;
int16_t y = attitude[Y];
if (y<0) y = -y;
int32_t result = (int32_t)x + y;
return result >= (long)(1.3 * INT16DEG_PI_FACTOR);
}
 
void flight_takeOff() {
void flight_setGround(void) {
resetIParts();
targetHeading = attitude[Z];
}
 
void flight_takeOff(void) {
// This is for GPS module to mark home position.
// TODO: What a disgrace, change it.
MKFlags |= MKFLAG_CALIBRATE;
// MKFlags |= MKFLAG_CALIBRATE;
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
compass_setTakeoffHeading(heading);
compass_setTakeoffHeading(attitude[YAW]);
#endif
}
 
83,22 → 100,16
/* Main Flight Control */
/************************************************************************/
void flight_control(void) {
int16_t tmp_int;
// Mixer Fractions that are combined for Motor Control
int16_t yawTerm, throttleTerm, term[2];
 
// PID controller variables
int16_t PDPart;
static int8_t debugDataTimer = 1;
float PID;
 
// High resolution motor values for smoothing of PID motor outputs
static int16_t motorFilters[MAX_MOTORS];
// static int16_t motorFilters[MAX_MOTORS];
 
uint8_t i, axis;
throttleTerm = controls[CONTROL_THROTTLE]; // in the -1000 to 1000 range there about.
 
throttleTerm = controls[CONTROL_THROTTLE];
 
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
if (throttleTerm > 200 && (MKFlags & MKFLAG_MOTOR_RUN)) {
// increment flight-time counter until overflow.
if (isFlying != 0xFFFF)
isFlying++;
110,204 → 121,94
*/
if (isFlying < 256) {
flight_setGround();
if (isFlying == 250)
if (isFlying == 255)
flight_takeOff();
}
 
// This check removed. Is done on a per-motor basis, after output matrix multiplication.
if (throttleTerm < staticParams.minThrottle + 10)
throttleTerm = staticParams.minThrottle + 10;
else if (throttleTerm > staticParams.maxThrottle - 20)
throttleTerm = (staticParams.maxThrottle - 20);
if (throttleTerm < minThrottle)
throttleTerm = minThrottle;
else if (throttleTerm > maxThrottle)
throttleTerm = maxThrottle;
 
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
// This is where control affects the target heading. It also (later) directly controls yaw.
targetHeading -= ((int32_t)controls[CONTROL_YAW] * YAW_STICK_INTEGRATION_SCALER_LSHIFT16) >> 16;
int16_t headingError;
 
// end part 1: 750-800 usec.
// start part 3: 350 - 400 usec.
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
// This is where control affects the target heading. It also (later) directly controls yaw.
headingError -= controls[CONTROL_YAW];
if (isHeadingUnaccountable()) {
// inverted flight. Attitude[Z] is off by PI and we should react in the oppisite direction!
debugOut.digital[0] |= DEBUG_INVERTED;
headingError = 0;
} else {
debugOut.digital[0] &= ~DEBUG_INVERTED;
headingError = attitude[Z] - targetHeading;
}
 
if (headingError < -YAW_I_LIMIT)
// Ahaa. Wait. Here is pretty much same check.
if (headingError < -YAW_I_LIMIT) {
headingError = -YAW_I_LIMIT;
else if (headingError > YAW_I_LIMIT)
targetHeading = attitude[Z] + YAW_I_LIMIT;
} else if (headingError > YAW_I_LIMIT) {
headingError = YAW_I_LIMIT;
 
PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
 
// Lets not limit P and D.
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
 
/*
* Compose yaw term.
* The yaw term is limited: Absolute value is max. = the throttle term / 2.
* However, at low throttle the yaw term is limited to a fixed value,
* and at high throttle it is limited by the throttle reserve (the difference
* between current throttle and maximum throttle).
*/
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
// Limit yawTerm
debugOut.digital[0] &= ~DEBUG_CLIP;
if (throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm / 2) {
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = -throttleTerm / 2;
} else if (yawTerm > throttleTerm / 2) {
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = throttleTerm / 2;
}
} else {
if (yawTerm < -MIN_YAWGAS / 2) {
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = -MIN_YAWGAS / 2;
} else if (yawTerm > MIN_YAWGAS / 2) {
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = MIN_YAWGAS / 2;
}
targetHeading = attitude[Z] - YAW_I_LIMIT;
}
//debugOut.analog[24] = targetHeading;
//debugOut.analog[25] = attitude[Z];
//debugOut.analog[26] = headingError;
//debugOut.analog[27] = positiveDynamic;
//debugOut.analog[28] = negativeDynamic;
 
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
// Yaw P part.
PID = ((int32_t)gyro_control[Z] * YAW_RATE_SCALER_LSHIFT16 * dynamicParams.yawGyroP) >> 16;
 
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
} else if (yawTerm > (tmp_int - throttleTerm)) {
yawTerm = (tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
if (flight_flightMode != FM_RATE) {
PID += ((int32_t)headingError * ATT_P_SCALER_LSHIFT16 * dynamicParams.yawGyroI) >> 16;
}
 
debugOut.digital[1] &= ~DEBUG_CLIP;
yawTerm = PID + controls[CONTROL_YAW];
// yawTerm = limitYawToThrottle(yawTerm);
 
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
/************************************************************************/
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis = PITCH; axis <= ROLL; axis++) {
PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
// In HH mode, the I part is summed from P and D of gyros minus stick.
if (gyroIFactor) {
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
//if (axis == 0) debugOut.analog[28] = iDiff;
PDPart += iDiff;
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
for (uint8_t axis = CONTROL_ROLL; axis <= CONTROL_PITCH; axis++) {
if (flight_flightMode == FM_RETURN_TO_LEVEL) {
// Control.
// The P part is attitude error.
PID = (((int32_t)attitude[axis] * ATT_P_SCALER_LSHIFT16 * dynamicParams.attGyroP) >> 16) + controls[axis];
// The I part is attitude error integral.
IPart[axis] += PID;
// The D part is rate.
PID += ((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.attGyroD) >> 16;
} else {
IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
// We want: Normal stick gain, full stick deflection should drive gyros halfway towards saturation.
// If that is not enough, then fully towards saturation.
PID = (((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.rateGyroP) >> 16) + controls[axis];
IPart[axis] += PID;
PID -= ((int32_t)gyroD[axis] * dynamicParams.rateGyroD) >> 8;
}
 
// So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
// attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
// We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
// That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
// So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
 
// With normal Ki, limit I parts to +/- 205 (of about 1024)
if (IPart[axis] < -64000) {
IPart[axis] = -64000;
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
} else if (IPart[axis] > 64000) {
IPart[axis] = 64000;
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
}
 
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
 
term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
// PDPart += (gyroD[axis] * (int16_t) dynamicParams.gyroD) / 16;
// Right now, let us ignore I.
// term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
term[axis] = PID;
term[axis] += (dynamicParams.levelCorrection[axis] - 128);
 
/*
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
* (max. pitch or roll term is the throttle value).
* OOPS: Is not applied at all.
* TODO: Why a growing function of yaw?
*/
if (term[axis] < -tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = -tmp_int;
} else if (term[axis] > tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = tmp_int;
}
debugOut.analog[12 + axis] = term[axis];
}
 
//debugOut.analog[14] = yawTerm;
//debugOut.analog[15] = throttleTerm;
//debugOut.analog[15] = IPart[0] / 1000;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
 
debugOut.analog[16] = acc[PITCH];
debugOut.analog[17] = acc[ROLL];
 
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
 
debugOut.analog[6] = getAngleEstimateFromAcc(PITCH) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
debugOut.analog[7] = getAngleEstimateFromAcc(ROLL) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
debugOut.analog[8] = acc[Z];
 
debugOut.analog[9] = controls[CONTROL_PITCH];
debugOut.analog[10] = controls[CONTROL_ROLL];
}
 
/*
debugOut.analog[6] = term[PITCH];
debugOut.analog[7] = term[ROLL];
debugOut.analog[8] = yawTerm;
debugOut.analog[9] = throttleTerm;
*/
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
uint8_t throttle;
 
tmp = (int32_t) throttleTerm * motorMixer.matrix[i][MIX_THROTTLE];
tmp += (int32_t) term[PITCH] * motorMixer.matrix[i][MIX_PITCH];
tmp += (int32_t) term[ROLL] * motorMixer.matrix[i][MIX_ROLL];
tmp += (int32_t) yawTerm * motorMixer.matrix[i][MIX_YAW];
tmp = tmp >> 6;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
// Now we scale back down to a 0..255 range.
tmp = motorFilters[i] / MOTOR_SCALING;
 
// So this was the THIRD time a throttle was limited. But should the limitation
// apply to the common throttle signal (the one used for setting the "power" of
// all motors together) or should it limit the throttle set for each motor,
// including mix components of pitch, roll and yaw? I think only the common
// throttle should be limited.
// --> WRONG. This caused motors to stall completely in tight maneuvers.
// Apply to individual signals instead.
CHECK_MIN_MAX(tmp, 1, 255);
throttle = tmp;
 
/*
if (i < 4)
debugOut.analog[10 + i] = throttle;
*/
 
if ((MKFlags & MKFLAG_MOTOR_RUN) && motorMixer.matrix[i][MIX_THROTTLE] > 0) {
motor[i].throttle = throttle;
} else if (motorTestActive) {
motor[i].throttle = motorTest[i];
} else {
motor[i].throttle = 0;
}
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
debugOut.analog[9] = controls[CONTROL_PITCH];
debugOut.analog[10] = controls[CONTROL_YAW];
debugOut.analog[11] = controls[CONTROL_THROTTLE];
}
/branches/dongfang_FC_rewrite/flight.h
6,19 → 6,55
#define _FLIGHT_H
 
#include <inttypes.h>
#include "analog.h"
#include "configuration.h"
#include "timer0.h"
 
#define PITCH 0
#define ROLL 1
#define YAW 2
typedef enum {
FM_UNINITALIZED,
FM_RETURN_TO_LEVEL,
FM_HEADING_HOLD,
FM_RATE
} FlightMode_t;
 
void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor);
// Max, yaw error to correct. If error is greater, the target heading is pulled towards current heading.
#define YAW_I_LIMIT (int)(PI / 3.0f * INT16DEG_PI_FACTOR)
 
// A PI/4 attitude error with average PFactor (PID_NORMAL_VALUE) should make a 1024<<16 diff.
// PI/4 * INT16DEG_PI_FACTOR * PID_NORMAL_VALUE * x = (CONTROL_INPUT_HARDWARE_RANGE<<16)/2 -->
// x = (CONTROL_INPUT_HARDWARE_RANGE<<15)*4 / (PID_NORMAL_VALUE * PI * INT16DEG_PI_FACTOR)
// x = CONTROL_INPUT_HARDWARE_RANGE<<17 / (PID_NORMAL_VALUE * PI * (1<<15) / PI)
// x = CONTROL_INPUT_HARDWARE_RANGE<<2 / PID_NORMAL_VALUE = about 82
#define ATT_P_SCALER_LSHIFT16 (int)(((1L<<(LOG_CONTROL_RANGE+2)) / PID_NORMAL_VALUE) + 0.5)
 
// A full control input (=half range) with average PFactor (PID_NORMAL_VALUE) should get a rate to drive gyros to PI/sec (say).
// PI/s * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2
// PI/s is PI * GYRO_RATE_FACTOR_XY units.
// PI * GYRO_RATE_FACTOR_XY * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2
// x = CONTROL_INPUT_HARDWARE_RANGE/(2*PI*GYRO_RATE_FACTOR_XY*PID_NORMAL_VALUE) = about 379
#define RATE_P_SCALER_LSHIFT16 (int)((1L<<(LOG_CONTROL_RANGE+16)) / (2*PI*GYRO_RATE_FACTOR_XY*PID_NORMAL_VALUE) + 0.5)
 
// A full control input (=half range) with average PFactor (PID_NORMAL_VALUE) should get a rate to drive gyros to PI/2sec (say).
// PI/2s * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2
// PI/2/s is PI/2 * GYRO_RATE_FACTOR_Z units.
// PI/2 * GYRO_RATE_FACTOR_Z * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2
// PI * GYRO_RATE_FACTOR_Z * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE
// x = CONTROL_INPUT_HARDWARE_RANGE / (PI * GYRO_RATE_FACTOR_Z * PID_NORMAL_VALUE)
// x<<16 is about 1517
#define YAW_RATE_SCALER_LSHIFT16 (int)((1L<<(LOG_CONTROL_RANGE+16)) / (PI*GYRO_RATE_FACTOR_Z*PID_NORMAL_VALUE) + 0.5)
 
// This is where control affects the target heading. It also (later) directly controls yaw.
// We want, at a normal yaw stick P, a PI/2s rate at full stick.
// That is, f_control * x * CONTROL_INPUT_HARDWARE_RANGE/2 = PI/2 * INT16DEG_PI_FACTOR= 1<<14
// f_control * x * CONTROL_INPUT_HARDWARE_RANGE = 1<<15
// x = 1<<15 / (CONTROL_INPUT_HARDWARE_RANGE * F_CONTROL)
// 4194
#define YAW_STICK_INTEGRATION_SCALER_LSHIFT16 (int)(((1L<<(15+16-LOG_CONTROL_RANGE)) / F_CONTROL) + 0.5)
 
void flight_setMode(FlightMode_t _flightMode);
void flight_setParameters(void);
 
void flight_setGround(void);
void flight_takeOff(void);
 
void flight_control(void);
void transmitMotorThrottleData(void);
// void flight_setNeutral(void);
 
#endif //_FLIGHT_H
/branches/dongfang_FC_rewrite/heightControl.c
1,14 → 1,9
#include <inttypes.h>
#include "analog.h"
#include "attitude.h"
#include "configuration.h"
#include "controlMixer.h"
// for digital / LED debug.
#include "output.h"
#include "definitions.h"
#include "debug.h"
 
// For scope debugging only!
#include "rc.h"
 
#define INTEGRAL_LIMIT 100000
 
#define LATCH_TIME 3
69,7 → 64,7
}
} else {
// Switch is not activated; take the "max-height" as the target height.
setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)
setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting));
}
/*
96,7 → 91,8
// uint8_t heightControlTestOscAmplitude;
 
hc_testOscPrescaler++;
if (hc_testOscPrescaler == 488) {
if (hc_testOscPrescaler == 250) {
hc_testOscPrescaler = 0;
hc_testOscTimer++;
if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
127,9 → 123,9
 
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
void HC_periodicTaskAndRPTY(int16_t* RPTY) {
HC_periodicTask();
int16_t throttle = PRTY[CONTROL_THROTTLE];
int16_t throttle = RPTY[CONTROL_THROTTLE];
int32_t height = analog_getHeight();
int32_t heightError = targetHeight - height;
int16_t dHeight = analog_getDHeight();
206,7 → 202,7
 
/* Experiment: Find hover-throttle */
 
#define DEFAULT_HOVERTHROTTLE 50
#define DEFAULT_HOVERTHROTTLE 200
int32_t stronglyFilteredHeightDiff = 0;
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
227,7 → 223,7
 
*/
PRTY[CONTROL_THROTTLE] = throttle;
RPTY[CONTROL_THROTTLE] = throttle;
}
 
/*
/branches/dongfang_FC_rewrite/heightControl.h
1,2 → 1,2
void HC_periodicTaskAndPRTY(int16_t* PRTY);
void HC_periodicTaskAndRPTY(int16_t* RPTY);
void HC_setGround(void);
/branches/dongfang_FC_rewrite/main.c
2,216 → 2,152
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
 
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "uart1.h"
//#include "uart1.h"
#include "output.h"
#include "menu.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
//#include "commands.h"
//#include "flight.h"
//#include "profiler.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "printf_P.h"
#include "twimaster.h"
#include "controlMixer.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
#include "eeprom.h"
#include "beeper.h"
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
#include "eeprom.h"
 
int16_t main(void) {
uint16_t timer;
uint16_t timer;
 
// disable interrupts global
cli();
// disable interrupts global
cli();
 
// analyze hardware environment
setCPUType();
setBoardRelease();
// analyze hardware environment
setCPUType();
setBoardRelease();
 
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
fdevopen(uart_putchar, NULL);
 
// This is strange: It should NOT be necessarty to do. But the call of the same,
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
channelMap_default();
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
 
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
analog_init();
I2C_init();
#ifdef USE_NAVICTRL
SPI_MasterInit();
#endif
// initalize modules
output_init();
timer0_init();
// timer2_init();
usart0_init();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
I2C_init();
analog_init();
#ifdef USE_MK3MAG
MK3MAG_init();
MK3MAG_init();
#endif
#ifdef USE_DIRECT_GPS
usart1_init();
usart1_init();
#endif
 
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
motorMixer_readOrDefault();
paramSet_readOrDefault();
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
outputMixer_readOrDefault();
paramSet_readOrDefault();
 
// enable interrupts global
sei();
// enable interrupts global
sei();
 
printf("\n\r===================================");
printf("\n\rFlightControl");
printf("\n\rHardware: Custom");
printf("\n\r CPU: Atmega644");
if (CPUType == ATMEGA644P)
printf("p");
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
printf("\n\r===================================");
printf("\n\r===================================");
printf("\n\rFlightControl");
printf("\n\rHardware: Custom");
printf("\n\r CPU: Atmega644");
if (CPUType == ATMEGA644P)
printf("p");
printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR,
VERSION_PATCH + 'a');
printf("\n\r===================================");
 
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
 
// Instead, while away the time by flashing the 2 outputs:
// First J16, then J17. Makes it easier to see which is which.
timer = setDelay(200);
outputSet(0,1);
GRN_OFF;
RED_ON;
while (!checkDelay(timer))
;
// Instead, while away the time by flashing the 2 outputs:
// First J16, then J17. Makes it easier to see which is which.
timer = setDelay(200);
output_setLED(0, 1);
GRN_OFF;
RED_ON;
while (!checkDelay(timer))
;
 
timer = setDelay(200);
outputSet(0,0);
outputSet(1,1);
RED_OFF;
GRN_ON;
while (!checkDelay(timer))
;
timer = setDelay(200);
output_setLED(0, 0);
output_setLED(1, 1);
RED_OFF;
GRN_ON;
while (!checkDelay(timer))
;
 
timer = setDelay(200);
while (!checkDelay(timer))
;
outputSet(1,0);
GRN_OFF;
timer = setDelay(200);
while (!checkDelay(timer))
;
output_setLED(1, 0);
GRN_OFF;
 
twi_diagnostics();
twi_diagnostics();
 
printf("\n\r===================================");
printf("\n\r===================================");
 
#ifdef USE_NAVICTRL
printf("\n\rSupport for NaviCtrl");
printf("\n\rSupport for NaviCtrl");
#endif
 
#ifdef USE_DIRECT_GPS
printf("\n\rDirect (no NaviCtrl) navigation");
printf("\n\rDirect (no NaviCtrl) navigation");
#endif
 
controlMixer_setNeutral();
controlMixer_setNeutral();
 
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
I2CTimeout = 5000;
 
// Init flight parameters
// flight_setNeutral();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
 
beep(2000);
// Init flight parameters
// flight_setNeutral();
 
printf("\n\rControl: ");
if (staticParams.bitConfig & CFG_HEADING_HOLD)
printf("Heading Hold");
else printf("RTL Mode");
beep(2000);
printf("\n\n\r");
 
printf("\n\n\r");
timer2_init();
/*
* Main loop updates attitude and does some nonessential tasks
* like beeping alarms and computing servo values.
*/
while (1) {
attitude_update();
 
LCD_clear();
// This is fair to leave here - servo values only need update after a change in attitude anyway.
// calculateServoValues();
 
I2CTimeout = 5000;
 
while (1) {
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
if (!analogDataReady) {
// Analog data should have been ready but is not!!
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
} else {
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 
J4HIGH;
// This is probably the correct order:
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
// Flight control uses results from both.
calculateFlightAttitude();
controlMixer_periodicTask();
commands_handleCommands();
flight_control();
J4LOW;
 
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
if (!I2CTimeout) {
I2C_Reset();
I2CTimeout = 5;
}
beepI2CAlarm();
}
// Allow serial data transmission if there is still time, or if we are not flying anyway.
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
usart0_transmitTxData();
}
 
usart0_processRxData();
 
if (checkDelay(timer)) {
if (UBat <= UBAT_AT_5V) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.batteryVoltageWarning) {
beepBatteryAlarm();
}
 
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
timer = setDelay(20); // every 20 ms
}
output_update();
}
 
#ifdef USE_NAVICTRL
if(!SendSPI) {
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
// within the SPI_TransmitByte() routine the value is set to 4.
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
SPI_TransmitByte();
}
#endif
 
calculateServoValues();
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
} else {
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
}
}
}
return (1);
if (UBat <= UBAT_AT_5V || UBat >= staticParams.batteryVoltageWarning) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
MKFlags &= ~MKFLAG_LOWBAT;
} else {
beepBatteryAlarm();
MKFlags |= MKFLAG_LOWBAT;
}
}
return 1;
}
/branches/dongfang_FC_rewrite/makefile
1,21 → 1,19
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
F_CPU = 20000000
F_CPU = 20000000UL
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MAJOR = 1
VERSION_MINOR = 1
VERSION_PATCH = 0
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version
 
#-------------------------------------------------------------------
#OPTIONS
 
# Use one of the extensions for a gps solution
#EXT = NAVICTRL
#EXT = DIRECT_GPS
#EXT = MK3MAG
EXT =
22,19 → 20,19
 
#GYRO=ENC-03_FC1.3
#GYRO_HW_NAME=ENC
#GYRO_HW_FACTOR=1.304f
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
#GYRO_HW_FACTOR=74.688f
#GYRO_XY_CORRECTION=1.2f
#GYRO_Z_CORRECTION=1.14f
 
GYRO=ADXRS610_FC2.0
GYRO_HW_NAME=ADXR
GYRO_HW_FACTOR=1.2288f
GYRO_PITCHROLL_CORRECTION=1.0f
GYRO_YAW_CORRECTION=1.0f
GYRO_HW_FACTOR=70.405f
GYRO_XY_CORRECTION=1.0f
GYRO_Z_CORRECTION=1.0f
 
#GYRO=invenSense
#GYRO_HW_NAME=Isense
#GYRO_HW_FACTOR=0.6827f
#GYRO_HW_FACTOR=39.114f
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
 
53,12 → 51,10
HEX_NAME = MEGA644p_$(EXT)_$(GYRO)
endif
 
 
ifeq ($(F_CPU), 20000000)
QUARZ = 20MHZ
endif
 
 
# Output format. (can be srec, ihex, binary)
FORMAT = ihex
 
73,54 → 69,6
ifeq ($(VERSION_PATCH), 2)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 3)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 4)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 5)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 6)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 7)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 8)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 9)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 10)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 11)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)l_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 12)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 13)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 14)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)o_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 15)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)p_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 16)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)q_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 17)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 100)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)df_SVN$(REV)
endif
 
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
129,10 → 77,12
 
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c failsafeControl.c
SRC += externalControl.c compassControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c
SRC += eeprom.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c
SRC = main.c uart0.c timer0.c timer2.c analog.c output.c controlMixer.c failsafeControl.c
SRC += externalControl.c compassControl.c twimaster.c rc.c flight.c profiler.c
SRC += eeprom.c heightControl.c configuration.c commands.c $(GYRO).c attitude_ardupilot.c
 
CPPSRC = AP_AHRS_DCM.cpp AP_Math.cpp Matrix3.cpp Vector3.cpp AP_Compass_HIL.cpp Compass.cpp
 
ifeq ($(EXT), DIRECT_GPS)
SRC += mk3mag.c directGPSNaviControl.c uart1.c ubx.c
endif
141,9 → 91,6
SRC += mk3mag.c
endif
 
ifeq ($(EXT), NAVICTRL)
SRC += spi.c
endif
##########################################################################################################
 
# List Assembler source files here.
169,11 → 116,15
# -ahlms: create assembler listing
CFLAGS = -O$(OPT) \
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \
-Wall -Wstrict-prototypes \
-Wall \
-DGYRO=$(GYRO) \
-Wa,-adhlns=$(<:.c=.lst) \
$(patsubst %,-I%,$(EXTRAINCDIRS))
 
CPPFLAGS = -O$(OPT) \
-funsigned-char -funsigned-bitfields -fshort-enums \
-Wall \
$(patsubst %,-I%,$(EXTRAINCDIRS))
 
# Set a "language standard" compiler flag.
# Unremark just one line below to set the language standard to use.
181,10 → 132,9
#CFLAGS += -std=c89
#CFLAGS += -std=gnu89
#CFLAGS += -std=c99
CFLAGS += -std=gnu99
#CFLAGS += -std=gnu99
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_SATURATION_RATE=${GYRO_SATURATION_RATE} -DGYRO_XY_CORRECTION=${GYRO_XY_CORRECTION} -DGYRO_Z_CORRECTION=${GYRO_Z_CORRECTION}
 
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION}
 
ifeq ($(EXT), DIRECT_GPS)
CFLAGS += -DUSE_DIRECT_GPS
CFLAGS += -DUSE_MK3MAG
194,10 → 144,6
CFLAGS += -DUSE_MK3MAG
endif
 
ifeq ($(EXT), NAVICTRL)
CFLAGS += -DUSE_NAVICTRL
endif
 
ifeq ($(SETUP), QUADRO)
CFLAGS += -DUSE_QUADRO
endif
221,8 → 167,6
# files -- see avr-libc docs [FIXME: not yet described there]
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs
 
 
 
# Optional linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
229,18 → 173,13
# --cref: add cross reference to map file
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
 
# Additional libraries
 
# Minimalistic printf version
#LDFLAGS += -Wl,-u,vfprintf -lprintf_min
 
# Floating point printf version (requires -lm below)
#LDFLAGS += -Wl,-u,vfprintf -lprintf_flt
 
# -lm = math library
LDFLAGS += -lm
LDFLAGS += -lc
LDFLAGS += -lm
 
 
##LDFLAGS += -T./linkerfile/avr5.x
 
 
302,6 → 241,8
SHELL = sh
 
CC = avr-gcc
#avr-gcc
CPPC = avr-g++
 
OBJCOPY = avr-objcopy
OBJDUMP = avr-objdump
331,15 → 272,19
MSG_SYMBOL_TABLE = Creating Symbol Table:
MSG_LINKING = Linking:
MSG_COMPILING = Compiling:
MSG_COMPILING_CPP = Compiling C++:
MSG_ASSEMBLING = Assembling:
MSG_CLEANING = Cleaning project:
 
 
# Define all object files.
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)
GENOBJ = $(CPPSRC:.cpp=.o) $(SRC:.c=.o) $(ASRC:.S=.o)
# OBJ = lib/square.o lib/_addsub_sf.o lib/_mul_sf.o $(GENOBJ)
OBJ = $(GENOBJ)
 
# Define all listing files.
LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)
# $(CPPSRC:.cpp=.lst)
 
# Combine all necessary flags and optional flags.
# Add target processor to flags.
346,13 → 291,12
#ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS)
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS)
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
ALL_CPPFLAGS = -mmcu=$(MCU) -I. $(CPPFLAGS)
 
 
# Default target.
all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \
$(TARGET).lss $(TARGET).sym sizeafter finished end
 
 
# Eye candy.
# AVR Studio 3.x does not check make's exit code but relies on
# the following magic strings to be generated by the compile job.
375,13 → 319,10
sizeafter:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
 
 
 
# Display compiler version information.
gccversion :
@$(CC) --version
 
 
# Convert ELF to COFF for use in debugging / simulating in
# AVR Studio or VMLAB.
COFFCONVERT=$(OBJCOPY) --debugging \
447,18 → 388,25
@echo $(MSG_LINKING) $@
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
 
 
# Compile: create object files from C source files.
%.o : %.c
@echo
@echo $(MSG_COMPILING) $<
$(CC) -c $(ALL_CFLAGS) $< -o $@
$(CPPC) -c $(ALL_CFLAGS) $< -o $@
 
# Compile: create object files from C source files.
%.o : %.cpp
@echo
@echo $(MSG_COMPILING_CPP) $<
$(CPPC) -c $(ALL_CPPFLAGS) $< -o $@
 
# Compile: create assembler files from C source files.
%.s : %.c
$(CC) -S $(ALL_CFLAGS) $< -o $@
 
# Compile: create assembler files from C++ source files.
%.s : %.cpp
$(CPPC) -S $(ALL_CFLAGS) $< -o $@
 
# Assemble: create object files from assembler source files.
%.o : %.S
466,11 → 414,6
@echo $(MSG_ASSEMBLING) $<
$(CC) -c $(ALL_ASFLAGS) $< -o $@
 
 
 
 
 
 
# Target: clean project.
clean: begin clean_list finished end
 
477,7 → 420,7
clean_list :
@echo
@echo $(MSG_CLEANING)
# $(REMOVE) $(TARGET).hex
$(REMOVE) $(TARGET).hex
$(REMOVE) $(TARGET).eep
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).cof
488,7 → 431,7
$(REMOVE) $(TARGET).sym
$(REMOVE) $(TARGET).lnk
$(REMOVE) $(TARGET).lss
$(REMOVE) $(OBJ)
$(REMOVE) $(GENOBJ)
$(REMOVE) $(LST)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRC:.c=.d)
/branches/dongfang_FC_rewrite/naviControl.h
4,6 → 4,6
#include<inttypes.h>
 
void navi_setNeutral(void);
void navigation_periodicTaskAndPRTY(int16_t* PRTY);
void navigation_periodicTaskAndRPTY(int16_t* RPTY);
 
#endif
/branches/dongfang_FC_rewrite/output.c
1,21 → 1,56
#include <inttypes.h>
#include "output.h"
#include "eeprom.h"
#include "debug.h"
#include "timer0.h"
#include "timer2.h"
#include "twimaster.h"
// For gimbal stab.
#include "attitude.h"
#include "definitions.h"
#include "flight.h"
#include "uart0.h"
#include "beeper.h"
#include "controlMixer.h"
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
uint8_t flashCnt[2], flashMask[2];
 
DebugOut_t debugOut;
int16_t throttleTerm;
int32_t yawTerm, term[2];
 
uint8_t positiveDynamic, negativeDynamic;
 
float previousManualValues[2];
 
void output_init(void) {
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1 << DDC2) | (1 << DDC3);
outputSet(0,0);
outputSet(1,0);
output_setLED(0,0);
output_setLED(1,0);
flashCnt[0] = flashCnt[1] = 0;
flashMask[0] = flashMask[1] = 128;
 
for (uint8_t axis=0; axis<2; axis++)
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * (1<<LOG_CONTROL_BYTE_SCALING);
}
 
void outputSet(uint8_t num, uint8_t state) {
void output_setParameters() {
if (staticParams.dynamicStability > PID_NORMAL_VALUE) {
// Normal gain of 1.
positiveDynamic = 1<<LOG_DYNAMIC_STABILITY_SCALER;
// Gain between 1 (for staticParams.dynamicStability == PID_NORMAL_VALUE) and 0(for staticParams.dynamicStability == 2*PID_NORMAL_VALUE)
negativeDynamic = (1<<(LOG_DYNAMIC_STABILITY_SCALER+1)) - (1<<LOG_DYNAMIC_STABILITY_SCALER) * staticParams.dynamicStability / PID_NORMAL_VALUE;
if (negativeDynamic < 0)
negativeDynamic = 0;
} else {
negativeDynamic = 1<<LOG_DYNAMIC_STABILITY_SCALER;
positiveDynamic = (1<<LOG_DYNAMIC_STABILITY_SCALER) * staticParams.dynamicStability / PID_NORMAL_VALUE;
}
 
}
 
void output_setLED(uint8_t num, uint8_t state) {
if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) {
if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num);
} else {
33,10 → 68,10
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) {
if (timing > 250 && manual > 230) {
// "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7.
outputSet(port, 1);
output_setLED(port, 1);
} else if (timing > 250 && manual < 10) {
// "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7.
outputSet(port, 0);
output_setLED(port, 0);
} else if (!flashCnt[port]--) {
// rotating mask over bitmask...
flashCnt[port] = timing - 1;
44,36 → 79,28
flashMask[port] = 128;
else
flashMask[port] >>= 1;
outputSet(port, flashMask[port] & bitmask);
output_setLED(port, flashMask[port] & bitmask);
}
}
 
void output_update(void) {
static int8_t delay = 0;
if (!delay--) { // 10 ms intervals
delay = 4;
}
if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) {
outputSet(0, 1);
outputSet(1, 1);
output_setLED(0, 1);
output_setLED(1, 1);
} else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) {
outputSet(0, 0);
outputSet(1, 0);
output_setLED(0, 0);
output_setLED(1, 0);
} else {
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) {
flashingLight(0, 25, 0x55, 25);
} else if (staticParams.outputDebugMask) {
outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask);
} else if (!delay) {
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing);
}
output_setLED(0, debugOut.digital[0] & staticParams.outputDebugMask);
} else flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing);
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) {
flashingLight(1, 25, 0x55, 25);
} else if (staticParams.outputDebugMask) {
outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask);
} else if (!delay) {
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing);
}
output_setLED(1, debugOut.digital[1] & staticParams.outputDebugMask);
} else flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing);
}
}
 
133,3 → 160,152
beepTime = 6000; // 0.6 seconds
}
}
 
// Result centered at 0 and scaled to control range steps.
float gimbalStabilizationPart(uint8_t axis) {
float value = attitude[axis];
//value *= STABILIZATION_FACTOR;
value *= ((float)CONTROL_RANGE / 50.0 / (1<<14)); // 1<<14 scales 90 degrees to full range at normal gain setting (50)
value *= staticParams.servoConfigurations[axis].stabilizationFactor;
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
return -value;
return value;
}
 
// Constant-speed limitation.
float gimbalManualPart(uint8_t axis) {
float manualValue = (dynamicParams.servoManualControl[axis] - 128) * (1<<LOG_CONTROL_BYTE_SCALING);
float diff = manualValue - previousManualValues[axis];
uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
if (diff > maxSpeed) diff = maxSpeed;
else if (diff < -maxSpeed) diff = -maxSpeed;
manualValue = previousManualValues[axis] + diff;
previousManualValues[axis] = manualValue;
return manualValue;
}
 
// Result centered at 0 and scaled in control range.
float gimbalServoValue(uint8_t axis) {
float value = gimbalStabilizationPart(axis);
value += gimbalManualPart(axis);
//int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
//if (value < limit) value = limit;
//limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
//if (value > limit) value = limit;
return value;
}
 
// Result centered at 0 and scaled in control range.
float getAuxValue(uint8_t auxSource) {
switch(auxSource) {
case (uint8_t)-1:
return 0;
case MIXER_SOURCE_AUX_GIMBAL_ROLL:
return gimbalServoValue(0);
case MIXER_SOURCE_AUX_GIMBAL_PITCH:
return gimbalServoValue(1);
default: // an R/C variable or channel or what we make of it...
return controls[auxSource - MIXER_SOURCE_AUX_RCCHANNEL];
}
}
 
// value is generally in the 10 bits range.
// mix is 6 bits.
// and dynamics are 6 bits --> 22 bits needed + sign + space to spare.
static inline int32_t mixin(int8_t mix, int16_t value) {
int32_t x = (int32_t)mix * value;
if (x > 0) {
return x * positiveDynamic;
} else {
return x * negativeDynamic;
}
}
 
void output_applyMulticopterMixer(void) {
int16_t _outputs[NUM_OUTPUTS];
 
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
_outputs[i] = 0;
}
 
// Process throttle, roll, pitch, yaw in special way with dynamic stability and with saturation to opposite motor.
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
if (outputMixer[i].outputType == OUTPUT_TYPE_MOTOR) {
int32_t tmp;
tmp = ((int32_t)throttleTerm<<6) * outputMixer[i].flightControls[MIXER_SOURCE_THROTTLE];
tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_ROLL], term[CONTROL_ROLL]);
tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_PITCH], term[CONTROL_PITCH]);
tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_YAW], yawTerm);
// Compensate for the factor of 64 multiplied by in matrix mixing and another factor of 64 for the positive/negative dynamic stuff.
_outputs[i] += tmp >> (LOG_MOTOR_MIXER_UNIT + LOG_DYNAMIC_STABILITY_SCALER);
 
// Deduct saturation from opposite motor output.
int16_t excess = _outputs[i] - (outputMixer[i].maxValue << LOG_CONTROL_BYTE_SCALING);
if (excess > 0) {
uint8_t oppositeIndex = outputMixer[i].oppositeMotor;
if (oppositeIndex != -1)
_outputs[oppositeIndex] -= excess;
}
}
}
 
// I2C part.
for (uint8_t i=0; i<MAX_I2CCHANNELS; i++) {
// I2C supports only motors anyway..
if (outputMixer[i].outputType != OUTPUT_TYPE_MOTOR) continue;
if (outputTestActive) {
mkblcs[i].throttle = outputTest[i];
} else if (MKFlags & MKFLAG_MOTOR_RUN) {
int16_t asByte = _outputs[i] >> LOG_CONTROL_BYTE_SCALING;
// Apply limits.
CHECK_MIN_MAX(asByte, outputMixer[i].minValue, outputMixer[i].maxValue);
if (i<4)
debugOut.analog[16 + i] = asByte;
mkblcs[i].throttle = asByte;
} else {
mkblcs[i].throttle = 0;
}
}
for (uint8_t i=0; i<MAX_PWMCHANNELS; i++) {
uint8_t sourceIndex = MAX_I2CCHANNELS + i;
 
if (outputMixer[sourceIndex].outputType == OUTPUT_TYPE_MOTOR) {
if (outputTestActive) {
// When testing, min/max does NOT apply.
pwmChannels[i] = (int16_t)(outputTest[sourceIndex] * PWM_BYTE_SCALE_FACTOR + PULSELENGTH_1000 + 0.5);
} else {
int16_t throttle;
if (MKFlags & MKFLAG_MOTOR_RUN) {
throttle = _outputs[sourceIndex];
int16_t min = outputMixer[sourceIndex].minValue << LOG_CONTROL_BYTE_SCALING;
int16_t max = outputMixer[sourceIndex].maxValue << LOG_CONTROL_BYTE_SCALING;
CHECK_MIN_MAX(throttle, min, max);
throttle = (int16_t)(throttle * PWM_CONTROL_SCALE_FACTOR + PULSELENGTH_1000 + 0.5);
} else {
throttle = PULSELENGTH_1000;
}
pwmChannels[i] = throttle;
}
} else if (outputMixer[sourceIndex].outputType == OUTPUT_TYPE_SERVO) {
int16_t servoValue;
if (outputTestActive) {
servoValue = outputTest[sourceIndex];
// When testing, min/max DOES apply.
CHECK_MIN_MAX(servoValue, outputMixer[sourceIndex].minValue, outputMixer[sourceIndex].maxValue);
servoValue = ((float)servoValue * PWM_BYTE_SCALE_FACTOR + PULSELENGTH_1000 + 0.5);
} else {
float fServoValue = getAuxValue(outputMixer[sourceIndex].auxSource);
int16_t min = (outputMixer[sourceIndex].minValue-128) << LOG_CONTROL_BYTE_SCALING;
int16_t max = (outputMixer[sourceIndex].maxValue-128) << LOG_CONTROL_BYTE_SCALING;
CHECK_MIN_MAX(fServoValue, min, max);
servoValue = (int16_t)(fServoValue * PWM_CONTROL_SCALE_FACTOR + PULSELENGTH_1500 + 0.5);
}
pwmChannels[i] = servoValue;
} else { // undefined channel.
pwmChannels[i] = PULSELENGTH_1500;
}
}
}
/branches/dongfang_FC_rewrite/output.h
2,73 → 2,21
#define _OUTPUT_H
 
#include <avr/io.h>
#include "configuration.h"
 
#define J3HIGH PORTD |= (1<<PORTD5)
#define J3LOW PORTD &= ~(1<<PORTD5)
#define J3TOGGLE PORTD ^= (1<<PORTD5)
 
#define J4HIGH PORTD |= (1<<PORTD4)
#define J4LOW PORTD &= ~(1<<PORTD4)
#define J4TOGGLE PORTD ^= (1<<PORTD4)
 
#define J5HIGH PORTD |= (1<<PORTD3)
#define J5LOW PORTD &= ~(1<<PORTD3)
#define J5TOGGLE PORTD ^= (1<<PORTD3)
 
// invert means: An "1" bit in digital debug data make a LOW on the output.
#define DIGITAL_DEBUG_INVERT 0
 
#define OUTPUT_HIGH(num) {PORTC |= (4 << (num));}
#define OUTPUT_LOW(num) {PORTC &= ~(4 << (num));}
#define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));}
 
/*
* Some digital debugs. A digital debug is 2 signals on the 2 LED outputs,
* turned on and off depending on some condtions given in the code.
* Only one can be selected, by defining DIGITAL_DEBUG_MASK to the value
* of the debug.
* In the code one can do like:
* if (whatever_condition) {
* DebugOut.Digital[0] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[0] &= ~DEBUG_MYOWNDEBUGGER;
* }
* ...
* if (whatever_other_condition) {
* DebugOut.Digital[1] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[1] &= ~DEBUG_MYOWNDEBUGGER;
* }
*
* Digital debugs may be added as desired, and removed when the mystery
* at hand is resolved.
*/
// Control terms (multicopter)
// TODO: Multicopter stuff separate?
extern int16_t throttleTerm;
extern int32_t yawTerm, term[2];
 
#define DEBUG_MAINLOOP_TIMER 1
#define DEBUG_HEIGHT_DIFF 2
#define DEBUG_FLIGHTCLIP 4
#define DEBUG_ACC0THORDER 8
#define DEBUG_COMPASS 16
#define DEBUG_PRESSURERANGE 32
#define DEBUG_CLIP 64
#define DEBUG_SENSORLIMIT 128
// I2C and PWM motor and servo outputs.
// extern int16_t outputs[NUM_OUTPUTS];
 
#define OUTPUTFLAGS_INVERT_0 1 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option)
#define OUTPUTFLAGS_INVERT_1 2 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option)
#define OUTPUTFLAGS_FLASH_0_AT_BEEP 4 // Flash LED when beeper beeps
#define OUTPUTFLAGS_FLASH_1_AT_BEEP 8 // Flash LED when beeper beeps
#define OUTPUTFLAGS_USE_ONBOARD_LEDS 16 // Control on-board LEDs in addition to outputs
#define OUTPUTFLAGS_TEST_OFF 32 // For testing: Turn off both outputs
#define OUTPUTFLAGS_TEST_ON 64 // For testing: Turn on both outputs
 
// For practical reasons put here instead of in uart0.h
typedef struct {
uint8_t digital[2];
uint16_t analog[32]; // debug values
}__attribute__((packed)) DebugOut_t;
 
extern DebugOut_t debugOut;
 
/*
* Set to 0 for using outputs as the usual flashing lights.
* Set to one of the DEBUG_... defines h for using the outputs as debug lights.
76,12 → 24,9
#define DIGITAL_DEBUG_MASK 0
 
void output_init(void);
void outputSet(uint8_t num, uint8_t state);
void output_setLED(uint8_t num, uint8_t state);
void output_update(void);
void beep(uint16_t millis);
void beepNumber(uint8_t numbeeps);
void beepRCAlarm(void);
void beepI2CAlarm(void);
void beepBatteryAlarm(void);
void output_applyMulticopterMixer(void);
void output_setParameters(void);
 
#endif //_output_H
/branches/dongfang_FC_rewrite/profiler.c
0,0 → 1,57
#include <inttypes.h>
#include <string.h>
#include "profiler.h"
 
uint8_t currentProfiledActivity;
volatile uint32_t totalProfilerHits;
volatile uint16_t activitiesTimerHits[16];
 
char profilerLabel0[] PROGMEM = "Unaccounted";
char profilerLabel1[] PROGMEM = "AnalogUpd";
char profilerLabel2[] PROGMEM = "MatrixUpd1";
char profilerLabel3[] PROGMEM = "MatrixUpd2";
char profilerLabel4[] PROGMEM = "Normalize1";
char profilerLabel5[] PROGMEM = "Normalize2";
char profilerLabel6[] PROGMEM = "DriftCorr";
char profilerLabel7[] PROGMEM = "CheckMatrix";
char profilerLabel8[] PROGMEM = "EulerAngles";
char profilerLabel9[] PROGMEM = "AnglesOutput";
char profilerLabel10[] PROGMEM = "ControlMixer";
char profilerLabel11[] PROGMEM = "Commands";
char profilerLabel12[] PROGMEM = "FlightControl";
char profilerLabel13[] PROGMEM = "UART";
char profilerLabel14[] PROGMEM = "Outputs";
char profilerLabel15[] PROGMEM = "";
 
PGM_P PROFILER_LABELS[] PROGMEM = {
profilerLabel0,
profilerLabel1,
profilerLabel2,
profilerLabel3,
profilerLabel4,
profilerLabel5,
profilerLabel6,
profilerLabel7,
profilerLabel8,
profilerLabel9,
profilerLabel10,
profilerLabel11,
profilerLabel12,
profilerLabel13,
profilerLabel14,
profilerLabel15
};
 
void setCurrentProfiledActivity(uint8_t what) {
currentProfiledActivity = what;
}
 
void reset(void) {
memset((uint8_t*)&activitiesTimerHits, 0, sizeof(activitiesTimerHits));
totalProfilerHits = 0;
}
 
void profiler_scoreTimerHit(void) {
activitiesTimerHits[currentProfiledActivity]++;
totalProfilerHits++;
}
/branches/dongfang_FC_rewrite/profiler.h
0,0 → 1,35
#ifndef _PROFILER_H
#define _PROFILER_H
 
#include <inttypes.h>
#include <avr/pgmspace.h>
 
#define UNACCOUNTED 0
 
#define ANALOG_UPDATE 1
#define MATRIX_UPDATE1 2
#define MATRIX_UPDATE2 3
#define MATRIX_NORMALIZE1 4
#define MATRIX_NORMALIZE2 5
#define DRIFT_CORRECTION 6
#define CHECK_MATRIX 7
#define EULER_ANGLES 8
#define ANGLESOUTPUT 9
 
#define CONTROLMIXER 10
#define COMMANDS 11
#define FLIGHTCONTROL 12
#define UART 13
#define OUTPUTS 14
 
//extern uint8_t currentProfiledActivity;
extern volatile uint16_t activitiesTimerHits[16];
extern volatile uint32_t totalProfilerHits;
void setCurrentProfiledActivity(uint8_t what);
void reset(void);
void profiler_scoreTimerHit(void);
 
extern PGM_P PROFILER_LABELS[] PROGMEM;
 
#endif
 
/branches/dongfang_FC_rewrite/rc.c
3,22 → 3,22
#include <avr/interrupt.h>
 
#include "rc.h"
#include "controlMixer.h"
//#include "controlMixer.h"
#include "configuration.h"
#include "commands.h"
#include "output.h"
#include "definitions.h"
 
// The channel array is 0-based!
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint16_t RC_buffer[MAX_CHANNELS];
volatile uint8_t inBfrPnt = 0;
volatile int16_t PPM_in[MAX_CONTROLCHANNELS];
volatile int16_t PPM_diff[MAX_CONTROLCHANNELS];
volatile uint16_t RC_buffer[MAX_CONTROLCHANNELS];
volatile uint8_t inBfrPnt;
 
volatile uint8_t RCQuality;
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t commandTimer = 0;
uint8_t lastRCCommand;
uint8_t commandTimer;
 
#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
#define TIME(s) ((int16_t)(((F_CPU/8000)*(float)s + 0.5f)))
 
/***************************************************************
* 16bit timer 1 is used to decode the PPM-Signal
48,13 → 48,13
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise cancler (bit: ICNC1=1)
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise canceler (bit: ICNC1=1)
// Therefore the counter increments at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
TCCR1B &= ~((1<<WGM13) | (1<<WGM12) | (1<<CS12));
TCCR1B |= (1<<CS11) | (1<<CS10) | (1<<ICNC1);
TCCR1B |= (1<<CS11) | (1<<ICNC1);
TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
 
if (channelMap.RCPolarity) {
71,9 → 71,7
// Enable Overflow Interrupt (bit: TOIE1=0)
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
TIMSK1 |= (1<<ICIE1);
 
RCQuality = 0;
 
SREG = sreg;
}
 
87,7 → 85,7
//sync gap? (3.5 ms < signal < 25.6 ms)
if (signal > TIME(3.5)) {
inBfrPnt = 0;
} else if (inBfrPnt<MAX_CHANNELS) {
} else if (inBfrPnt<MAX_CONTROLCHANNELS) {
RC_buffer[inBfrPnt++] = signal;
if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
}
117,12 → 115,12
 
void RC_process(void) {
if (RCQuality) RCQuality--;
for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
for (uint8_t channel=0; channel<MAX_CONTROLCHANNELS; channel++) {
uint16_t signal = RC_buffer[channel];
if (signal != 0) {
RC_buffer[channel] = 0; // reset to flag value already used.
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
signal -= TIME(1.5);
signal -= TIME(1.5) /* + channelMap.HWTrim */;
PPM_diff[channel] = signal - PPM_in[channel];
PPM_in[channel] = signal;
}
132,7 → 130,7
 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
#define COMMAND_THRESHOLD 85
#define COMMAND_THRESHOLD TIME(0.35f)
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
160,24 → 158,30
/*
* Get Pitch, Roll, Throttle, Yaw values
*/
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
void RC_periodicTaskAndRPTY(int16_t* RPTY) {
int16_t tmp1, tmp2;
RC_process();
if (RCQuality) {
RCQuality--;
PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
RPTY[CONTROL_ROLL] = ((RCChannel(CH_ROLL) * staticParams.stickP) >> 3) + RCDiff(CH_ROLL) * staticParams.stickD;
RPTY[CONTROL_PITCH] = ((RCChannel(CH_PITCH) * staticParams.stickP) >> 3) + RCDiff(CH_PITCH) * staticParams.stickD;
int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + TIME(0.4);
// Negative throttle values are taken as zero.
if (throttle > 0)
PRTY[CONTROL_THROTTLE] = throttle;
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
RPTY[CONTROL_THROTTLE] = throttle;
else
RPTY[CONTROL_THROTTLE] = 0;
 
tmp1 = RCChannel(CH_YAW); // - RCDiff(CH_YAW);
// exponential stick sensitivity in yawing rate
tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 2;
PRTY[CONTROL_YAW] = tmp2;
//
tmp2 = ((int32_t)staticParams.stickYawP * (int32_t)tmp1 * abs(tmp1)) >> 14; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 3;
RPTY[CONTROL_YAW] = (/*(RCChannel(CH_YAW) * staticParams.stickYawP) >> 3*/ tmp2);
 
uint8_t command = RC_getStickCommand();
 
if (lastRCCommand == command) {
// Keep timer from overrunning.
if (commandTimer < COMMAND_TIMER)
196,12 → 200,12
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return RCChannel(varNum + CH_POTS) + POT_OFFSET;
return (RCChannel(varNum + CH_VARIABLES) >> 3) + VARIABLES_OFFSET;
/*
* Let's just say:
* The RC variable i is hardwired to channel i, i>=4
*/
return PPM_in[varNum] + POT_OFFSET;
return (PPM_in[varNum] >> 3) + VARIABLES_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
261,7 → 265,7
* Not in any of these positions: 0
*/
 
#define ARGUMENT_THRESHOLD 70
#define ARGUMENT_THRESHOLD TIME(0.35f)
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
 
/branches/dongfang_FC_rewrite/rc.h
5,11 → 5,11
#include "configuration.h"
 
// Number of cycles a command must be repeated before commit.
#define COMMAND_TIMER 200
#define COMMAND_TIMER 100
 
extern void RC_Init(void);
// the RC-Signal. todo: Not export any more.
extern volatile int16_t PPM_in[MAX_CHANNELS];
extern volatile int16_t PPM_in[MAX_CONTROLCHANNELS];
// extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported??
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame
extern volatile uint8_t RCQuality; // rc signal quality indicator (0 to 200)
19,11 → 19,11
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_POTS 4
#define POT_OFFSET 120
#define CH_VARIABLES 4
#define VARIABLES_OFFSET 120
 
// void RC_periodicTask(void);
void RC_periodicTaskAndPRTY(int16_t* PRTY);
void RC_periodicTaskAndRPTY(int16_t* RPTY);
uint8_t RC_getArgument(void);
uint8_t RC_getCommand(void);
int16_t RC_getVariable(uint8_t varNum);
/branches/dongfang_FC_rewrite/timer0.c
1,25 → 1,33
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
//#include "eeprom.h"
#include "profiler.h"
#include "controlMixer.h"
#include "configuration.h"
#include "analog.h"
#include "controlMixer.h"
 
#include "timer0.h"
#include "debug.h"
#include "beeper.h"
#include "output.h"
#include "commands.h"
#include "flight.h"
#include "uart0.h"
#include "twimaster.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
 
volatile uint32_t globalMillisClock = 0;
volatile uint8_t runFlightControl = 0;
volatile uint16_t beepTime = 0;
#define MILLIS_DIVIDER 10
 
volatile uint32_t jiffiesClock;
volatile uint32_t millisClock;
volatile uint8_t loopJiffiesClock;
volatile uint16_t beepTime;
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
 
#ifdef USE_NAVICTRL
volatile uint8_t SendSPI = 0;
#endif
volatile uint8_t flightControlStatus;
 
/*****************************************************
* Initialize Timer 0
27,144 → 35,184
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
void timer0_init(void) {
uint8_t sreg = SREG;
uint8_t sreg = SREG;
 
// disable all interrupts before reconfiguration
cli();
// disable all interrupts before reconfiguration
cli();
 
// Configure speaker port as output.
if (boardRelease == 10) { // Speaker at PD2
DDRD |= (1 << DDD2);
PORTD &= ~(1 << PORTD2);
} else { // Speaker at PC7
DDRC |= (1 << DDC7);
PORTC &= ~(1 << PORTC7);
}
// Configure speaker port as output.
if (boardRelease == 10) { // Speaker at PD2
DDRD |= (1 << DDD2);
PORTD &= ~(1 << PORTD2);
} else { // Speaker at PC7
DDRC |= (1 << DDC7);
PORTC &= ~(1 << PORTC7);
}
 
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
DDRB |= (1 << DDB4) | (1 << DDB3);
PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
DDRB |= (1 << DDB4) | (1 << DDB3);
PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
 
// Timer/Counter 0 Control Register A
// Timer/Counter 0 Control Register A
 
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
 
// Timer/Counter 0 Control Register B
// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
// Timer/Counter 0 Control Register B
// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
 
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
 
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
// init Timer/Counter 0 Register
TCNT0 = 0;
 
// init Timer/Counter 0 Register
TCNT0 = 0;
// Timer/Counter 0 Interrupt Mask Register
// enable timer overflow interrupt only
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
TIMSK0 |= (1 << TOIE0);
 
// Timer/Counter 0 Interrupt Mask Register
// enable timer overflow interrupt only
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
TIMSK0 |= (1 << TOIE0);
SREG = sreg;
}
 
SREG = sreg;
void runFlightControlTask(void) {
if (flightControlStatus != NOT_RUNNING) {
// Previous execution not completed! It is dangerous to start another.
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
return;
}
 
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 
controlMixer_periodicTask();
commands_handleCommands();
flightControlStatus = RUNNING;
 
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
if (!I2CTimeout) {
I2C_reset();
I2CTimeout = 5;
}
beepI2CAlarm();
}
if (analog_controlDataStatus != CONTROL_SENSOR_DATA_READY) {
// Analog data should have been ready but is not!!
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
} else {
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
J4HIGH;
analog_sumAttitudeData();
analog_updateControlData();
flight_control();
output_applyMulticopterMixer();
I2C_start(TWI_STATE_MOTOR_TX);
J4LOW;
}
 
flightControlStatus = NOT_RUNNING;
}
 
/*****************************************************/
/* Interrupt Routine of Timer 0 */
/*****************************************************/
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
static uint8_t cnt_1ms = 1, cnt = 0;
uint8_t beeperOn = 0;
ISR (TIMER0_OVF_vect) { // 9765.625 Hz
static uint8_t millisDivider = MILLIS_DIVIDER;
static uint8_t controlLoopDivider = CONTROLLOOP_DIVIDER;
static uint8_t serialLoopDivider = SERIALLOOP_DIVIDER /2;
static uint8_t outputLoopDivider = OUTPUTLOOP_DIVIDER /3;
uint8_t beeperOn = 0;
 
#ifdef USE_NAVICTRL
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
#endif
jiffiesClock++;
loopJiffiesClock++;
// profiler_scoreTimerHit();
 
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
cnt = 9;
cnt_1ms ^= 1;
if (!cnt_1ms) {
if (runFlightControl == 1)
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
else
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
sei();
 
if (!--millisDivider) {
millisClock++;
millisDivider = MILLIS_DIVIDER;
}
 
if (!--controlLoopDivider) {
//sei();
controlLoopDivider= CONTROLLOOP_DIVIDER;
runFlightControlTask();
//cli();
}
 
if (!--serialLoopDivider) {
//sei();
serialLoopDivider= SERIALLOOP_DIVIDER;
// Allow serial data transmission if there is still time, or if we are not flying anyway.
usart0_transmitTxData();
usart0_processRxData();
//cli();
}
globalMillisClock++; // increment millisecond counter
}
 
// beeper on if duration is not over
if (beepTime) {
beepTime--; // decrement BeepTime
if (beepTime & beepModulation)
beeperOn = 1;
else
beeperOn = 0;
} else { // beeper off if duration is over
beeperOn = 0;
beepModulation = BEEP_MODULATION_NONE;
}
if (!--outputLoopDivider) {
//sei();
outputLoopDivider= OUTPUTLOOP_DIVIDER;
output_update();
//cli();
}
 
if (beeperOn) {
// set speaker port to high.
if (boardRelease == 10)
PORTD |= (1 << PORTD2); // Speaker at PD2
else
PORTC |= (1 << PORTC7); // Speaker at PC7
} else { // beeper is off
// set speaker port to low
if (boardRelease == 10)
PORTD &= ~(1 << PORTD2);// Speaker at PD2
else
PORTC &= ~(1 << PORTC7);// Speaker at PC7
}
// beeper on if duration is not over
if (beepTime) {
beepTime--; // decrement BeepTime
if (beepTime & beepModulation)
beeperOn = 1;
else
beeperOn = 0;
} else { // beeper off if duration is over
beeperOn = 0;
beepModulation = BEEP_MODULATION_NONE;
}
 
if (beeperOn) {
// set speaker port to high.
if (boardRelease == 10)
PORTD |= (1 << PORTD2); // Speaker at PD2
else
PORTC |= (1 << PORTC7); // Speaker at PC7
} else { // beeper is off
// set speaker port to low
if (boardRelease == 10)
PORTD &= ~(1 << PORTD2);// Speaker at PD2
else
PORTC &= ~(1 << PORTC7);// Speaker at PC7
}
 
#ifdef USE_MK3MAG
// update compass value if this option is enabled in the settings
if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
MK3MAG_periodicTask(); // read out mk3mag pwm
}
// update compass value if this option is enabled in the settings
if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
MK3MAG_periodicTask(); // read out mk3mag pwm
}
#endif
}
 
// -----------------------------------------------------------------------
uint16_t setDelay(uint16_t t) {
return (globalMillisClock + t - 1);
return (millisClock + t - 1);
}
 
// -----------------------------------------------------------------------
int8_t checkDelay(uint16_t t) {
return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
return (((t - millisClock) & 0x8000) >> 8); // check sign bit
}
 
// -----------------------------------------------------------------------
void delay_ms(uint16_t w) {
uint16_t t_stop = setDelay(w);
while (!checkDelay(t_stop))
;
uint16_t t_stop = setDelay(w);
while (!checkDelay(t_stop))
;
}
 
// -----------------------------------------------------------------------
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
uint16_t t_stop;
t_stop = setDelay(w);
while (!checkDelay(t_stop)) {
if (analogDataReady) {
analog_update();
startAnalogConversionCycle();
}
}
if (stop) {
// Wait for new samples to get prepared but do not restart AD conversion after that!
// Caller MUST to that.
while (!analogDataReady);
}
}
/branches/dongfang_FC_rewrite/timer0.h
5,28 → 5,28
 
// Normally it is 20MHz/2048 = 9765.625 Hz
#define F_TIMER0IRQ ((float)F_CPU/2048.0)
#define MAINLOOP_DIVIDER 10
// Originally it is 488
#define F_MAINLOOP (F_TIMER0IRQ/(2.0*MAINLOOP_DIVIDER))
#define T_TIMER0IRQ ((float)2048.0/(float)F_CPU)
// About 250 Hz
#define F_CONTROL 250
#define CONTROLLOOP_DIVIDER (F_TIMER0IRQ/F_CONTROL)
#define SERIALLOOP_DIVIDER 61
#define OUTPUTLOOP_DIVIDER 100
 
#define BEEP_MODULATION_NONE 0xFFFF
#define BEEP_MODULATION_RCALARM 0x0C00
#define BEEP_MODULATION_I2CALARM 0x0080
#define BEEP_MODULATION_BATTERYALARM 0x0300
#define BEEP_MODULATION_EEPROMALARM 0x0007
extern volatile uint32_t jiffiesClock;
extern volatile uint32_t millisClock;
extern volatile uint8_t loopJiffiesClock;
 
extern volatile uint32_t globalMillisClock;
extern volatile uint8_t runFlightControl;
extern volatile uint16_t beepModulation;
extern volatile uint16_t beepTime;
#ifdef USE_NAVICTRL
extern volatile uint8_t SendSPI;
#endif
 
extern void timer0_init(void);
extern void delay_ms(uint16_t w);
extern void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop);
extern uint16_t setDelay(uint16_t t);
extern int8_t checkDelay(uint16_t t);
 
typedef enum {
NOT_RUNNING = 0,
RUNNING,
BLOCKED_FOR_CALIBRATION
} FlightControlRunStatus;
 
extern volatile uint8_t flightControlStatus;
 
#endif //_TIMER0_H
/branches/dongfang_FC_rewrite/timer2.c
1,36 → 1,18
#include <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
#include "rc.h"
#include "attitude.h"
#include "timer2.h"
 
#define COARSERESOLUTION 1
 
#ifdef COARSERESOLUTION
#define NEUTRAL_PULSELENGTH 938
#define STABILIZATION_LOG_DIVIDER 6
#define SERVOLIMIT 500
#define SCALE_FACTOR 4
#if (SERVO_RESOLUTION == COARSE)
#define CS2 ((1<<CS21)|(1<<CS20))
 
#else
 
#define NEUTRAL_PULSELENGTH 3750
#define STABILIZATION_LOG_DIVIDER 4
#define SERVOLIMIT 2000
#define SCALE_FACTOR 16
#define CS2 (1<<CS21)
#endif
 
#define MAX_SERVOS 8
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128)
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
#define FRAMELEN (PULSELENGTH_2200 * staticParams.servoCount + 128)
 
//volatile uint8_t servoActive = 0;
volatile uint8_t recalculateServoTimes = 0;
volatile uint16_t servoValues[MAX_SERVOS];
volatile uint16_t previousManualValues[2];
// volatile uint8_t servoActive = 0;
// volatile uint8_t recalculateServoTimes = 0;
volatile uint16_t pwmChannels[MAX_PWMCHANNELS];
 
#define HEF4017R_ON PORTC |= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
78,9 → 60,6
TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
TIMSK2 |= (1 << OCIE2A);
 
for (uint8_t axis=0; axis<2; axis++)
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
 
SREG = sreg;
}
 
97,52 → 76,8
/*****************************************************
* Control Servo Position
*****************************************************/
int16_t calculateStabilizedServoAxis(uint8_t axis) {
int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
// With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
// That is a divisor of about 1<<14. Same conclusion as H&I.
value *= staticParams.servoConfigurations[axis].stabilizationFactor;
value = value >> 8;
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
return -value;
return value;
}
 
// With constant-speed limitation.
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
int16_t diff = manualValue - previousManualValues[axis];
uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
if (diff > maxSpeed) diff = maxSpeed;
else if (diff < -maxSpeed) diff = -maxSpeed;
manualValue = previousManualValues[axis] + diff;
previousManualValues[axis] = manualValue;
return manualValue;
}
 
// add stabilization and manual, apply soft position limits.
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
int16_t featuredServoValue(uint8_t axis) {
int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR);
value += calculateStabilizedServoAxis(axis);
int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
if (value < limit) value = limit;
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
if (value > limit) value = limit;
return value;
}
 
uint16_t servoValue(uint8_t axis) {
int16_t value;
if (axis<2) value = featuredServoValue(axis);
else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8.
// Shift out of the [0..255*SCALE_FACTOR] space
value -= (128 * SCALE_FACTOR);
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
else if (value > SERVOLIMIT) value = SERVOLIMIT;
// Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
return value + NEUTRAL_PULSELENGTH;
}
 
/*
void calculateServoValues(void) {
if (!recalculateServoTimes) return;
for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
150,9 → 85,8
}
recalculateServoTimes = 0;
}
*/
 
uint8_t servoMap[] = {2,3,0,1,4,5,6,7};
 
ISR(TIMER2_COMPA_vect) {
static uint16_t remainingPulseTime;
static uint8_t servoIndex = 0;
162,17 → 96,16
// Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
if (servoIndex < staticParams.servoCount) {
// There are more signals to output.
sumOfPulseTimes += (remainingPulseTime = servoValues[servoMap[servoIndex]]);
sumOfPulseTimes += (remainingPulseTime = pwmChannels[servoIndex]); //pwmChannels[servoMap[servoIndex]]);
servoIndex++;
} else {
// There are no more signals. Reset the counter and make this pulse cover the missing frame time.
remainingPulseTime = FRAMELEN - sumOfPulseTimes;
sumOfPulseTimes = servoIndex = 0;
recalculateServoTimes = 1;
HEF4017R_ON;
}
}
 
// Schedule the next OCR2A event. The counter is already reset at this time.
if (remainingPulseTime > 256+128) {
// Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
179,18 → 112,18
// as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
OCR2A = 255;
TCCR2A &= ~(1<<COM2A0);
remainingPulseTime-=256;
remainingPulseTime -= 256;
} else if (remainingPulseTime > 256) {
// Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
// way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
// condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
remainingPulseTime-=128;
OCR2A=127;
remainingPulseTime -= 128;
} else {
// Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low
TCCR2A |= (1<<COM2A0);
OCR2A = remainingPulseTime-1;
remainingPulseTime=0;
remainingPulseTime = 0;
HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0.
}
}
/branches/dongfang_FC_rewrite/timer2.h
2,8 → 2,37
#define _TIMER2_H
 
#include <inttypes.h>
#include <math.h>
#include "configuration.h"
 
extern volatile uint16_t pwmChannels[MAX_PWMCHANNELS];
 
#define SERVO_RESOLUTION FINE
 
#if (SERVO_RESOLUTION == COARSE)
#define F_TIMER2 (F_CPU/32)
#define CS2 ((1<<CS21)|(1<<CS20))
#else
#define F_TIMER2(F_CPU/8)
#define CS2 (1<<CS21)
#endif
 
//#define ABSOLUTE_MIN ((float)F_TIMER2*0.00075 + 0.5)
//#define ABSOLUTE_MAX ((float)F_TIMER2*0.00225 + 0.5)
 
#define PULSELENGTH_800 ((float)F_TIMER2*0.0008)
#define PULSELENGTH_1000 ((float)F_TIMER2*0.0010)
#define PULSELENGTH_1500 ((float)F_TIMER2*0.0015)
#define PULSELENGTH_2000 ((float)F_TIMER2*0.0020)
#define PULSELENGTH_2200 ((float)F_TIMER2*0.0022)
 
#define PWM_CONTROL_SCALE_FACTOR (PULSELENGTH_1000/(float)CONTROL_RANGE)
#define PWM_BYTE_SCALE_FACTOR (PULSELENGTH_1000/256.0)
 
// Prevent damage even if grossly misconfigured:
// Multiply this by angle in int16-degrees and again by 100 to get servo deflection to same angle
// (assuming 90 degrees over 1 ms, which is the norm):
 
void timer2_init(void);
void calculateServoValues(void);
//void calculateServoValues(void);
#endif //_TIMER2_H
 
/branches/dongfang_FC_rewrite/twimaster.c
2,22 → 2,21
#include <avr/interrupt.h>
#include <util/twi.h>
#include <util/delay.h>
#include <stdio.h>
#include "twimaster.h"
#include "configuration.h"
//#include "configuration.h"
//#include "controlMixer.h"
#include "analog.h"
#include "printf_P.h"
 
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX;
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint8_t twi_state;
volatile uint8_t dac_channel;
volatile uint8_t writeIndex;
volatile uint8_t readIndex;
volatile uint16_t I2CTimeout = 100;
uint8_t missingMotor = 0;
uint8_t missingMotor;
MLBLC_t mkblcs[MAX_I2CCHANNELS];
uint8_t DACChannel;
 
motorData_t motor[MAX_MOTORS];
 
uint8_t DACChannel = 0;
 
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
 
41,16 → 40,15
TWSR &= ~((1 << TWPS1) | (1 << TWPS0));
 
// set TWI Bit Rate Register
TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2;
TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;
 
twi_state = TWI_STATE_MOTOR_TX;
motor_write = 0;
motor_read = 0;
writeIndex = 0;
readIndex = 0;
 
for (i = 0; i < MAX_MOTORS; i++) {
motor[i].throttle = 0;
motor[i].present = 0;
motor[i].maxPWM = 0;
for (i = 0; i < MAX_I2CCHANNELS; i++) {
mkblcs[i].present = 0;
mkblcs[i].maxPWM = 0;
}
 
SREG = sreg;
59,7 → 57,7
/****************************************
* Start I2C
****************************************/
void I2C_Start(uint8_t start_state) {
void I2C_start(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
75,7 → 73,7
/****************************************
* Stop I2C
****************************************/
void I2C_Stop(uint8_t start_state) {
void I2C_stop(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
91,7 → 89,7
/****************************************
* Write to I2C
****************************************/
void I2C_WriteByte(int8_t byte) {
void I2C_writeByte(int8_t byte) {
// move byte to send into TWI Data Register
TWDR = byte;
// clear interrupt flag (TWINT = 1)
103,7 → 101,7
/****************************************
* Receive byte and send ACK
****************************************/
void I2C_ReceiveByte(void) {
void I2C_receiveByte(void) {
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA);
}
 
110,7 → 108,7
/****************************************
* I2C receive last byte and send no ACK
****************************************/
void I2C_ReceiveLastByte(void) {
void I2C_receiveLastByte(void) {
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);
}
 
117,13 → 115,13
/****************************************
* Reset I2C
****************************************/
void I2C_Reset(void) {
void I2C_reset(void) {
// stop i2c bus
I2C_Stop(TWI_STATE_MOTOR_TX);
I2C_stop(TWI_STATE_MOTOR_TX);
twi_state = 0;
motor_write = TWDR;
motor_write = 0;
motor_read = 0;
writeIndex = TWDR;
writeIndex = 0;
readIndex = 0;
TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
131,7 → 129,7
TWSR = 0;
TWBR = 0;
I2C_init();
I2C_Start(TWI_STATE_MOTOR_TX);
I2C_start(TWI_STATE_MOTOR_TX);
}
 
/****************************************
144,83 → 142,84
// Master Transmit
case 0: // TWI_STATE_MOTOR_TX
// skip motor if not used in mixer
while ((motorMixer.matrix[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS))
motor_write++;
if (motor_write >= MAX_MOTORS) { // writing finished, read now
motor_write = 0;
while ((outputMixer[writeIndex].outputType != OUTPUT_TYPE_MOTOR) && (writeIndex < MAX_I2CCHANNELS))
writeIndex++;
if (writeIndex >= MAX_I2CCHANNELS) { // writing finished, read now
writeIndex = 0;
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode
I2C_writeByte(0x53 + (readIndex * 2)); // select slave adress in rx mode
} else
I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode
I2C_writeByte(0x52 + (writeIndex * 2)); // select slave adress in tx mode
break;
case 1: // Send Data to Slave
I2C_WriteByte(motor[motor_write].throttle); // transmit throttle value.
//I2C_writeByte(outputs[writeIndex]>>LOG_CONTROL_OUTPUT_SCALING); // transmit throttle value.
I2C_writeByte(mkblcs[writeIndex].throttle); // transmit throttle value.
break;
case 2: // repeat case 0+1 for all motors
if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received
if (!missing_motor)
missing_motor = motor_write + 1;
if (++motor[motor_write].error == 0)
motor[motor_write].error = 255; // increment error counter and handle overflow
missing_motor = writeIndex + 1;
if (++mkblcs[writeIndex].error == 0)
mkblcs[writeIndex].error = 255; // increment error counter and handle overflow
}
I2C_Stop(TWI_STATE_MOTOR_TX);
I2C_stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write++; // next motor
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
writeIndex++; // next motor
I2C_start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
break;
// Master Receive Data
case 3:
if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received
// no response from the addressed slave received
motor[motor_read].present = 0;
motor_read++; // next motor
if (motor_read >= MAX_MOTORS)
motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
mkblcs[readIndex].present = 0;
readIndex++; // next motor
if (readIndex >= MAX_I2CCHANNELS)
readIndex = 0; // restart reading of first motor if we have reached the last one
I2C_stop(TWI_STATE_MOTOR_TX);
} else {
motor[motor_read].present = ('1' - '-') + motor_read;
I2C_ReceiveByte(); //Transmit 1st byte
mkblcs[readIndex].present = ('1' - '-') + readIndex;
I2C_receiveByte(); //Transmit 1st byte
}
missingMotor = missing_motor;
missing_motor = 0;
break;
case 4: //Read 1st byte and transmit 2nd Byte
motor[motor_read].current = TWDR;
I2C_ReceiveLastByte(); // nack
mkblcs[readIndex].current = TWDR;
I2C_receiveLastByte(); // nack
break;
case 5:
//Read 2nd byte
motor[motor_read].maxPWM = TWDR;
motor_read++; // next motor
if (motor_read >= MAX_MOTORS)
motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
mkblcs[readIndex].maxPWM = TWDR;
readIndex++; // next motor
if (readIndex >= MAX_I2CCHANNELS)
readIndex = 0; // restart reading of first motor if we have reached the last one
I2C_stop(TWI_STATE_MOTOR_TX);
break;
 
// Writing ADC values.
case 7:
I2C_WriteByte(0x98); // Address the DAC
I2C_writeByte(0x98); // Address the DAC
break;
 
case 8:
I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
I2C_writeByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
break;
 
case 9:
I2C_WriteByte(gyroAmplifierOffset.offsets[DACChannel]);
I2C_writeByte(gyroAmplifierOffset.offsets[DACChannel]);
break;
 
case 10:
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80
I2C_writeByte(0x80); // 2nd byte for all channels is 0x80
break;
 
case 11:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2C_stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if (DACChannel < 2) {
DACChannel++; // jump to next channel
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
I2C_start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
} else {
DACChannel = 0; // reset dac channel counter
}
227,10 → 226,10
break;
 
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2C_stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
writeIndex = 0;
readIndex = 0;
}
}
 
240,25 → 239,25
 
printf("\n\rFound BL-Ctrl: ");
 
for (i = 0; i < MAX_MOTORS; i++) {
motor[i].throttle = 0;
for (i=0; i<MAX_I2CCHANNELS; i++) {
mkblcs[i].throttle = 0;
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
I2C_start(TWI_STATE_MOTOR_TX);
_delay_ms(2);
 
motor_read = 0; // read the first I2C-Data
readIndex = 0; // read the first I2C-Data
 
for (i = 0; i < MAX_MOTORS; i++) {
I2C_Start(TWI_STATE_MOTOR_TX);
for (i = 0; i < MAX_I2CCHANNELS; i++) {
I2C_start(TWI_STATE_MOTOR_TX);
_delay_ms(2);
if (motor[i].present)
if (mkblcs[i].present)
printf("%d ",i+1);
}
 
for (i = 0; i < MAX_MOTORS; i++) {
if (!motor[i].present && motorMixer.matrix[i][MIX_THROTTLE] > 0)
for (i = 0; i < MAX_I2CCHANNELS; i++) {
if (!mkblcs[i].present && outputMixer[i].outputType == OUTPUT_TYPE_MOTOR)
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1);
motor[i].error = 0;
mkblcs[i].error = 0;
}
}
/branches/dongfang_FC_rewrite/twimaster.h
12,21 → 12,21
extern uint8_t missingMotor;
 
typedef struct {
uint8_t throttle; // written by attitude controller
uint8_t throttle;
uint8_t current;
uint8_t maxPWM;
uint8_t present; // 0 if BL was found
uint8_t error; // I2C error counter
uint8_t current; // read back from BL
uint8_t maxPWM; // read back from BL
}__attribute__((packed)) motorData_t;
}__attribute__((packed)) MLBLC_t;
 
extern motorData_t motor[MAX_MOTORS];
extern MLBLC_t mkblcs[MAX_I2CCHANNELS];
 
extern volatile uint16_t I2CTimeout;
 
extern void I2C_init(void); // Initialize I2C
extern void I2C_Start(uint8_t start_state); // Start I2C
extern void I2C_Stop(uint8_t start_state); // Stop I2C
extern void I2C_Reset(void); // Reset I2C
extern void I2C_start(uint8_t startState); // Start I2C
extern void I2C_stop(uint8_t startState); // Stop I2C
extern void I2C_reset(void); // Reset I2C
extern void twi_diagnostics(void);
 
#endif
/branches/dongfang_FC_rewrite/uart0.c
6,14 → 6,13
#include <string.h>
 
#include "eeprom.h"
#include "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "debug.h"
#include "profiler.h"
#include "beeper.h"
 
#ifdef USE_DIRECT_GPS
#include "mk3mag.h"
26,19 → 25,23
#define FALSE 0
#define TRUE 1
 
uint8_t requestedDebugLabel = 255;
DebugOut_t debugOut;
 
uint8_t request_verInfo = FALSE;
uint8_t request_externalControl = FALSE;
uint8_t request_display = FALSE;
uint8_t request_display1 = FALSE;
uint8_t request_debugData = FALSE;
uint8_t request_data3D = FALSE;
uint8_t request_PPMChannels = FALSE;
uint8_t request_motorTest = FALSE;
uint8_t request_variables = FALSE;
uint8_t request_OSD = FALSE;
uint8_t requestedAnalogLabel = 255;
uint8_t requestedProfilerLabel = 255;
 
uint8_t request_verInfo;
uint8_t request_externalControl;
uint8_t request_display;
uint8_t request_display1;
uint8_t request_debugData;
uint8_t request_profilerData;
uint8_t request_PPMChannels;
uint8_t request_outputTest;
uint8_t request_variables;
uint8_t request_OSD;
uint8_t request_DCM_matrix;
 
/*
#define request_verInfo (1<<0)
#define request_externalControl (1<<1)
54,18 → 57,16
 
//uint16_t request = 0;
 
uint8_t displayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t receivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t rxDataLen = 0;
volatile uint8_t txd_complete;
volatile uint8_t receivedBytes;
volatile uint8_t *pRxData;
volatile uint8_t rxDataLen;
 
uint8_t motorTestActive = 0;
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t outputTestActive;
uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t confirmFrame;
 
typedef struct {
72,14 → 73,15
int16_t heading;
}__attribute__((packed)) Heading_t;
 
Data3D_t data3D;
IMUData imuData;
 
uint16_t debugData_timer;
uint16_t data3D_timer;
uint16_t OSD_timer;
uint16_t debugData_interval = 0; // in 1ms
uint16_t data3D_interval = 0; // in 1ms
uint16_t OSD_interval = 0;
uint16_t debugDataTimer;
uint16_t profilerDataTimer;
uint16_t OSDDataTimer;
uint16_t debugDataInterval; // in 1ms
uint16_t profilerDataInterval; // in 1ms
uint16_t imuDataInterval; // in 1ms
uint16_t OSDDataInterval;
 
#ifdef USE_DIRECT_GPS
int16_t toMk3MagTimer;
86,48 → 88,81
#endif
 
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] = {
//1234567890123456
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"AccPitch(0.1deg)",
"AccRoll(0.1deg) ",
"AccZ ",
"RC pitch ",
"RC roll ", //10
" ",
"zerothOrderCorr ",
"DriftCompPitch ",
"DriftCompRoll ",
"GActivityDivider", //15
"AccPitch ",
"AccRoll ",
"CorrectionSum pi",
"CorrectionSum ro",
"control act wghd", //20
"acc vector wghd ",
"Height[dm] ",
"dHeight ",
"acc vector ",
"EFT ", //25
"naviPitch ",
"naviRoll ",
"Rate Tolerance ",
"Gyro Act Cont. ",
"GPS altitude ", //30
"GPS vert accura "
};
//1234567890123456
const char analogLabel0[] PROGMEM = "AngleRoll";
const char analogLabel1[] PROGMEM = "AnglePitch";
const char analogLabel2[] PROGMEM = "AngleYaw";
const char analogLabel3[] PROGMEM = "GyroX(Roll)";
const char analogLabel4[] PROGMEM = "GyroY(Pitch)";
const char analogLabel5[] PROGMEM = "GyroZ(Yaw)";
const char analogLabel6[] PROGMEM = "AccX(0.01m/s^2)";
const char analogLabel7[] PROGMEM = "AccY(0.01m/s^2)";
const char analogLabel8[] PROGMEM = "AccZ(0.01m/s^2)";
const char analogLabel9[] PROGMEM = "RC pitch";
const char analogLabel10[] PROGMEM = "RC yaw";
const char analogLabel11[] PROGMEM = "RC throttle";
const char analogLabel12[] PROGMEM = "Roll";
const char analogLabel13[] PROGMEM = "Pitch";
const char analogLabel14[] PROGMEM = "rollControl";
const char analogLabel15[] PROGMEM = "pitchControl";
const char analogLabel16[] PROGMEM = "M1";
const char analogLabel17[] PROGMEM = "M2";
const char analogLabel18[] PROGMEM = "M3";
const char analogLabel19[] PROGMEM = "M4";
const char analogLabel20[] PROGMEM = "flightmode";
const char analogLabel21[] PROGMEM = "Att freq";
const char analogLabel22[] PROGMEM = "Height[dm]";
const char analogLabel23[] PROGMEM = "dHeight";
const char analogLabel24[] PROGMEM = "attitudeSumCount";
const char analogLabel25[] PROGMEM = "simpleAirPressure";
const char analogLabel26[] PROGMEM = "OCR0A";
const char analogLabel27[] PROGMEM = "filteredAirPressure";
const char analogLabel28[] PROGMEM = "height";
const char analogLabel29[] PROGMEM = "Gyro Act Cont.";
const char analogLabel30[] PROGMEM = "GPS altitude";
const char analogLabel31[] PROGMEM = "GPS vert accura";
 
PGM_P ANALOG_LABELS[] PROGMEM = {
analogLabel0,
analogLabel1,
analogLabel2,
analogLabel3,
analogLabel4,
analogLabel5,
analogLabel6,
analogLabel7,
analogLabel8,
analogLabel9,
analogLabel10,
analogLabel11,
analogLabel12,
analogLabel13,
analogLabel14,
analogLabel15,
analogLabel16,
analogLabel17,
analogLabel18,
analogLabel19,
analogLabel20,
analogLabel21,
analogLabel22,
analogLabel23,
analogLabel24,
analogLabel25,
analogLabel26,
analogLabel27,
analogLabel28,
analogLabel29,
analogLabel30,
analogLabel31
};
 
/****************************************************************/
/* Initialization of the USART0 */
/****************************************************************/
void usart0_init(void) {
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
uint16_t ubrr = (F_CPU / (8 * USART0_BAUD) - 1);
 
// disable all interrupts before configuration
cli();
180,7 → 215,8
UCSR0B |= (1 << TXCIE0);
 
// initialize the debug timer
debugData_timer = setDelay(debugData_interval);
debugDataTimer = setDelay(debugDataInterval);
profilerDataTimer = setDelay(profilerDataInterval);
 
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
240,11 → 276,11
return; // if rxd buffer is locked immediately return
 
// the rxd buffer is unlocked
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and synchronization character is received
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
checksum = c; // init checksum
}
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incoming bytes
if (c != '\r') { // no termination character
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
checksum += c; // update checksum
295,9 → 331,9
 
// --------------------------------------------------------------------------
// application example:
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
// sendData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
/*
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
va_list ap;
uint16_t txd_bufferIndex = 0;
uint8_t *currentBuffer;
344,7 → 380,7
}
*/
 
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
va_list ap;
uint16_t pt = 0;
uint8_t a, b, c;
447,12 → 483,13
 
// --------------------------------------------------------------------------
void usart0_processRxData(void) {
// We control the motorTestActive var from here: Count it down.
if (motorTestActive)
motorTestActive--;
// We control the outputTestActive var from here: Count it down.
if (outputTestActive)
outputTestActive--;
// if data in the rxd buffer are not locked immediately return
if (!rxd_buffer_locked)
return;
 
uint8_t tempchar[3];
decode64(); // decode data block in rxd_buffer
 
468,27 → 505,24
break;
#endif
case 't': // motor test
if (rxDataLen > 20) {
memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest));
} else {
memcpy(&motorTest[0], (uint8_t*) pRxData, 4);
}
motorTestActive = 255;
memcpy(&outputTest[0], (uint8_t*) pRxData, /*sizeof(outputTest)*/ 12); // 12 is an mktool limitation.
outputTestActive = 255;
// Huh??
externalControlActive = 255;
break;
 
case 'n':// Read motor mixer
tempchar[0] = EEMIXER_REVISION;
tempchar[1] = sizeof(MotorMixer_t);
tempchar[1] = sizeof(OutputMixer_t);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&motorMixer, sizeof(motorMixer));
sendData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&outputMixer, sizeof(OutputMixer_t));
break;
 
case 'm':// "Set Mixer Table
if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(MotorMixer_t))) {
memcpy(&motorMixer, (uint8_t*)&pRxData[2], sizeof(motorMixer));
motorMixer_writeToEEProm();
if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(OutputMixer_t))) {
memcpy(&outputMixer, (uint8_t*)&pRxData[2], sizeof(OutputMixer_t));
outputMixer_writeToEEProm();
while (!txd_complete)
; // wait for previous frame to be sent
tempchar[0] = 1;
495,7 → 529,7
} else {
tempchar[0] = 0;
}
sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
sendData('M', FC_ADDRESS, 1, &tempchar, 1);
break;
 
case 'p': // get PPM channels
507,7 → 541,7
tempchar[1] = sizeof(IMUConfig);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
sendData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
break;
 
case 'j':// Save IMU configuration
522,7 → 556,7
}
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('J', FC_ADDRESS, 1, &tempchar, 1);
sendData('J', FC_ADDRESS, 1, &tempchar, 1);
}
break;
 
544,7 → 578,7
tempchar[2] = sizeof(staticParams);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
sendData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
break;
 
case 's': // save settings
559,11 → 593,11
tempchar[0] = getActiveParamSet();
beepNumber(tempchar[0]);
} else {
tempchar[0] = 0; //indicate bad data
tempchar[0] = sizeof(staticParams); //indicate bad data
}
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
sendData('S', FC_ADDRESS, 1, &tempchar, 1);
}
break;
 
575,32 → 609,42
default: // any Slave Address
switch (rxd_buffer[2]) {
case 'a':// request for labels of the analog debug outputs
requestedDebugLabel = pRxData[0];
if (requestedDebugLabel > 31)
requestedDebugLabel = 31;
requestedAnalogLabel = pRxData[0];
if (requestedAnalogLabel > 31)
requestedAnalogLabel = 31;
break;
 
case 'b': // submit extern control
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(ExternalControl_t));
confirmFrame = externalControl.frame;
externalControlActive = 255;
break;
 
case 'h':// request for display columns
remoteKeys |= pRxData[0];
if (remoteKeys)
displayLine = 0;
request_display = TRUE;
break;
case 'd': // request for the debug data
debugDataInterval = (uint16_t) pRxData[0] * 10;
if (debugDataInterval > 0)
request_debugData = TRUE;
break;
 
case 'l':// request for display columns
menuItem = pRxData[0];
request_display1 = TRUE;
break;
case 'e': // Requeset for the DCM matrix
request_DCM_matrix = TRUE;
break;
 
case 'f':
requestedProfilerLabel = pRxData[0];
if (requestedProfilerLabel > 15)
requestedProfilerLabel = 15;
break;
 
case 'u':
profilerDataInterval = (uint16_t) pRxData[0] * 10;
if (profilerDataInterval > 0)
request_profilerData = TRUE;
break;
 
case 'o':// request for OSD data (FC style)
OSD_interval = (uint16_t) pRxData[0] * 10;
if (OSD_interval > 0)
OSDDataInterval = (uint16_t) pRxData[0] * 10;
if (OSDDataInterval > 0)
request_OSD = TRUE;
break;
616,18 → 660,6
request_externalControl = TRUE;
break;
 
case 'd': // request for the debug data
debugData_interval = (uint16_t) pRxData[0] * 10;
if (debugData_interval > 0)
request_debugData = TRUE;
break;
 
case 'c': // request for the 3D data
data3D_interval = (uint16_t) pRxData[0] * 10;
if (data3D_interval > 0)
request_data3D = TRUE;
break;
 
default:
//unsupported command received
break;
643,9 → 675,9
/************************************************************************/
/* Routine f�r die Serielle Ausgabe */
/************************************************************************/
int16_t uart_putchar(int8_t c) {
int uart_putchar(char c, FILE* fims) {
if (c == '\n')
uart_putchar('\r');
uart_putchar('\r', fims);
// wait until previous character was send
loop_until_bit_is_set(UCSR0A, UDRE0);
// send character
659,58 → 691,61
return;
 
if (request_verInfo && txd_complete) {
sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
sendData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
request_verInfo = FALSE;
}
 
if (request_display && txd_complete) {
LCD_printMenu();
sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine),
&displayBuff[displayLine * 20], 20);
displayLine++;
if (displayLine >= 4)
displayLine = 0;
request_display = FALSE;
}
 
if (request_display1 && txd_complete) {
LCD_printMenu();
sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem,
sizeof(maxMenuItem), displayBuff, sizeof(displayBuff));
request_display1 = FALSE;
}
 
if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
uint8_t label[16]; // local sram buffer
memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
sizeof(requestedDebugLabel), label, 16);
requestedDebugLabel = 0xFF;
if (requestedAnalogLabel != 0xFF && txd_complete) {
char label[17]; // local sram buffer
memset(label, ' ', sizeof(label));
strcpy_P(label, (PGM_P)pgm_read_word(&(ANALOG_LABELS[requestedAnalogLabel]))); // read label from flash to sram buffer
sendData('A', FC_ADDRESS, 2, (uint8_t *) &requestedAnalogLabel, sizeof(requestedAnalogLabel), label, 16);
requestedAnalogLabel = 0xFF;
}
 
if (requestedProfilerLabel != 0xFF && txd_complete) {
char label[17]; // local sram buffer
memset(label, ' ', sizeof(label));
strcpy_P(label, (PGM_P)pgm_read_word(&(PROFILER_LABELS[requestedProfilerLabel]))); // read label from flash to sram buffer
sendData('F', FC_ADDRESS, 2, (uint8_t *) &requestedProfilerLabel, sizeof(requestedProfilerLabel), label, 16);
requestedProfilerLabel = 0xFF;
}
 
if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
sendData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
confirmFrame = 0;
}
 
if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData)
&& txd_complete) {
sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
debugData_timer = setDelay(debugData_interval);
if (((debugDataInterval && checkDelay(debugDataTimer)) || request_debugData) && txd_complete) {
sendData('D', FC_ADDRESS, 1, (uint8_t *)&debugOut, sizeof(debugOut));
debugDataTimer = setDelay(debugDataInterval);
request_debugData = FALSE;
}
 
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
data3D_timer = setDelay(data3D_interval);
request_data3D = FALSE;
if (((profilerDataInterval && checkDelay(profilerDataTimer)) || request_profilerData) && txd_complete) {
sendData('U', FC_ADDRESS, 2, (uint8_t *)&totalProfilerHits, sizeof(totalProfilerHits), (uint8_t *)&activitiesTimerHits, sizeof(activitiesTimerHits));
profilerDataTimer = setDelay(profilerDataInterval);
request_profilerData = FALSE;
}
 
if (request_DCM_matrix && txd_complete) {
/*
sendData('E', FC_ADDRESS, 1,
(uint8_t *) &dcmGyro, sizeof(dcmGyro));
*/
request_DCM_matrix = FALSE;
}
 
if (request_externalControl && txd_complete) {
sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
sendData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
sizeof(externalControl));
request_externalControl = FALSE;
}
722,7 → 757,7
toMk3Mag.userParam[0] = dynamicParams.userParams[0];
toMk3Mag.userParam[1] = dynamicParams.userParams[1];
toMk3Mag.calState = compassCalState;
sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
sendData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
// the last state is 5 and should be send only once to avoid multiple flash writing
if(compassCalState > 4) compassCalState = 0;
toMk3MagTimer = setDelay(99);
729,19 → 764,19
}
#endif
 
if (request_motorTest && txd_complete) {
sendOutData('T', FC_ADDRESS, 0);
request_motorTest = FALSE;
if (request_outputTest && txd_complete) {
sendData('T', FC_ADDRESS, 0);
request_outputTest = FALSE;
}
 
if (request_PPMChannels && txd_complete) {
uint8_t length = MAX_CHANNELS;
sendOutData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
uint8_t length = MAX_CONTROLCHANNELS;
sendData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
request_PPMChannels = FALSE;
}
 
if (request_variables && txd_complete) {
sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
sendData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
request_variables = FALSE;
}
 
751,7 → 786,7
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
sendData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
OSD_timer = setDelay(OSD_interval);
request_OSD = FALSE;
}
/branches/dongfang_FC_rewrite/uart0.h
1,10 → 1,11
#ifndef _UART0_H
#define _UART0_H
 
#define TXD_BUFFER_LEN 150
#define RXD_BUFFER_LEN 150
#define TXD_BUFFER_LEN 140
#define RXD_BUFFER_LEN 140
 
#include <inttypes.h>
#include <stdio.h>
#include "ubx.h"
 
//Baud rate of the USART
13,22 → 14,31
extern void usart0_init(void);
extern void usart0_transmitTxData(void);
extern void usart0_processRxData(void);
extern int16_t uart_putchar(int8_t c);
//extern int16_t uart_putchar(int8_t c);
 
extern int uart_putchar(char c, FILE* fims);
 
// extern uint8_t remotePollDisplayLine;
 
extern uint8_t motorTestActive;
extern uint8_t motorTest[16];
extern uint8_t outputTestActive;
extern uint8_t outputTest[16];
 
typedef struct {
int16_t anglePitch; // in 0.1 deg
int16_t angleRoll; // in 0.1 deg
int16_t heading; // in 0.1 deg
uint8_t reserved[8];
}__attribute__((packed)) Data3D_t;
float pitchRate; // in radians
float rollRate; // in radians
float yawRate; // in radians
 
float pitch; // in radians
float roll; // in radians
float heading; // in radians
 
float xAcc;
float yAcc;
float zAcc;
}__attribute__((packed)) IMUData;
 
typedef struct {
Data3D_t attitude;
IMUData imuData;
GPS_INFO_t GPSInfo;
int32_t airpressureHeight;
int16_t batteryVoltage;
/branches/dongfang_FC_rewrite/uart1.c
11,7 → 11,7
void usart1_init(void) {
// USART1 Control and Status Register A, B, C and baud rate register
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU / (8 * USART1_BAUD) - 1);
uint16_t ubrr = (uint16_t) (F_CPU / (8 * USART1_BAUD) - 1);
 
// disable all interrupts before reconfiguration
cli();
/branches/dongfang_FC_rewrite/ubx.c
74,8 → 74,8
UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 
volatile uint8_t GPSTimeout = 0;
volatile uint16_t GPSDatasetCounter = 0;
volatile uint8_t GPSTimeout;
volatile uint16_t GPSDatasetCounter;
 
void updateGPSInfo(void) {
if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {
/branches/dongfang_FC_rewrite/vector3d.h
0,0 → 1,72
#ifndef VECTOR3D__H
#define VECTOR3D__H
 
//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
static float R;
R = vector[0]*vector[0];
R += vector[1]*vector[1];
R += vector[2]*vector[2];
return sqrt(R);
}
 
//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
static float R;
R = vector3d_modulus(vector);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}
 
//calcuate vector dot-product c = a . b
float vector3d_dot(float* a,float* b){
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
 
 
//calcuate vector cross-product c = a x b
void vector3d_cross(float* a,float* b, float* c){
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}
 
//calcuate vector scalar-product n = s x a
void vector3d_scale(float s, float* a , float* b){
b[0] = s*a[0];
b[1] = s*a[1];
b[2] = s*a[2];
}
 
 
//calcuate vector sum c = a + b
void vector3d_add(float* a , float* b, float* c){
c[0] = a[0] + b[0];
c[1] = a[1] + b[1];
c[2] = a[2] + b[2];
}
 
 
//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
// {z,1,-x}
// {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m){
m[0*3+0]=1;
m[0*3+1]=-v[2];
m[0*3+2]=v[1];
m[1*3+0]=v[2];
m[1*3+1]=1;
m[1*3+2]=-v[0];
m[2*3+0]=-v[1];
m[2*3+1]=v[0];
m[2*3+2]=1;
}
 
 
 
 
 
#endif