/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 |