mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Merge remote-tracking branch 'origin/next' into amorale/OP-1058_uavo_named_accessors
This commit is contained in:
commit
11b269f898
@ -69,6 +69,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
|
||||
/**
|
||||
* Update the PID computation with setpoint weighting on the derivative
|
||||
* @param[in] pid The PID struture which stores temporary information
|
||||
* @param[in] factor A dynamic factor to scale pid's by, to compensate nonlinearities
|
||||
* @param[in] setpoint The setpoint to use
|
||||
* @param[in] measured The measured value of output
|
||||
* @param[in] dT The time step
|
||||
@ -77,12 +78,12 @@ float pid_apply(struct pid *pid, const float err, float dT)
|
||||
* This version of apply uses setpoint weighting for the derivative component so the gain
|
||||
* on the gyro derivative can be different than the gain on the setpoint derivative
|
||||
*/
|
||||
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT)
|
||||
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT)
|
||||
{
|
||||
float err = setpoint - measured;
|
||||
|
||||
// Scale up accumulator by 1000 while computing to avoid losing precision
|
||||
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
|
||||
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
|
||||
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
|
||||
|
||||
// Calculate DT1 term,
|
||||
@ -90,11 +91,11 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas
|
||||
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
|
||||
pid->lastErr = (deriv_gamma * setpoint - measured);
|
||||
if (pid->d > 0.0f && dT > 0.0f) {
|
||||
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
|
||||
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
|
||||
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
|
||||
} // 7.9577e-3 means 20 Hz f_cutoff
|
||||
|
||||
return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm;
|
||||
return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -44,7 +44,7 @@ struct pid {
|
||||
|
||||
// ! Methods to use the pid structures
|
||||
float pid_apply(struct pid *pid, const float err, float dT);
|
||||
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT);
|
||||
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
|
||||
void pid_zero(struct pid *pid);
|
||||
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
|
||||
void pid_configure_derivative(float cutoff, float gamma);
|
||||
|
@ -249,6 +249,7 @@ static bool NMEA_parse_real(int32_t *whole, uint32_t *fract, uint8_t *fract_unit
|
||||
PIOS_DEBUG_Assert(whole);
|
||||
PIOS_DEBUG_Assert(fract);
|
||||
PIOS_DEBUG_Assert(fract_units);
|
||||
PIOS_DEBUG_Assert(field);
|
||||
|
||||
field_w = strsep(&s, ".");
|
||||
field_f = s;
|
||||
@ -274,9 +275,6 @@ static float NMEA_real_to_float(char *nmea_real)
|
||||
uint32_t fract;
|
||||
uint8_t fract_units;
|
||||
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(nmea_real);
|
||||
|
||||
if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) {
|
||||
return false;
|
||||
}
|
||||
|
@ -149,36 +149,41 @@ static void systemTask(__attribute__((unused)) void *parameters)
|
||||
struct rfm22b_stats radio_stats;
|
||||
PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
|
||||
|
||||
// Update the status
|
||||
oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
|
||||
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
||||
oplinkStatus.RxGood = radio_stats.rx_good;
|
||||
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
|
||||
oplinkStatus.RxErrors = radio_stats.rx_error;
|
||||
oplinkStatus.RxMissed = radio_stats.rx_missed;
|
||||
oplinkStatus.RxFailure = radio_stats.rx_failure;
|
||||
oplinkStatus.TxDropped = radio_stats.tx_dropped;
|
||||
oplinkStatus.TxResent = radio_stats.tx_resent;
|
||||
oplinkStatus.TxFailure = radio_stats.tx_failure;
|
||||
oplinkStatus.Resets = radio_stats.resets;
|
||||
oplinkStatus.Timeouts = radio_stats.timeouts;
|
||||
oplinkStatus.RSSI = radio_stats.rssi;
|
||||
oplinkStatus.LinkQuality = radio_stats.link_quality;
|
||||
if (first_time) {
|
||||
first_time = false;
|
||||
if (pios_rfm22b_id) {
|
||||
// Update the status
|
||||
oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
|
||||
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
||||
oplinkStatus.RxGood = radio_stats.rx_good;
|
||||
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
|
||||
oplinkStatus.RxErrors = radio_stats.rx_error;
|
||||
oplinkStatus.RxMissed = radio_stats.rx_missed;
|
||||
oplinkStatus.RxFailure = radio_stats.rx_failure;
|
||||
oplinkStatus.TxDropped = radio_stats.tx_dropped;
|
||||
oplinkStatus.TxResent = radio_stats.tx_resent;
|
||||
oplinkStatus.TxFailure = radio_stats.tx_failure;
|
||||
oplinkStatus.Resets = radio_stats.resets;
|
||||
oplinkStatus.Timeouts = radio_stats.timeouts;
|
||||
oplinkStatus.RSSI = radio_stats.rssi;
|
||||
oplinkStatus.LinkQuality = radio_stats.link_quality;
|
||||
if (first_time) {
|
||||
first_time = false;
|
||||
} else {
|
||||
uint16_t tx_count = radio_stats.tx_byte_count;
|
||||
uint16_t rx_count = radio_stats.rx_byte_count;
|
||||
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
|
||||
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
|
||||
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
prev_tx_count = tx_count;
|
||||
prev_rx_count = rx_count;
|
||||
}
|
||||
oplinkStatus.TXSeq = radio_stats.tx_seq;
|
||||
oplinkStatus.RXSeq = radio_stats.rx_seq;
|
||||
oplinkStatus.LinkState = radio_stats.link_state;
|
||||
} else {
|
||||
uint16_t tx_count = radio_stats.tx_byte_count;
|
||||
uint16_t rx_count = radio_stats.rx_byte_count;
|
||||
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
|
||||
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
|
||||
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
prev_tx_count = tx_count;
|
||||
prev_rx_count = rx_count;
|
||||
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
|
||||
}
|
||||
oplinkStatus.TXSeq = radio_stats.tx_seq;
|
||||
oplinkStatus.RXSeq = radio_stats.rx_seq;
|
||||
oplinkStatus.LinkState = radio_stats.link_state;
|
||||
|
||||
if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
|
||||
LINK_LED_ON;
|
||||
} else {
|
||||
|
@ -41,6 +41,7 @@
|
||||
#include "relaytuningsettings.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "attitudestate.h"
|
||||
#include "airspeedstate.h"
|
||||
#include "gyrostate.h"
|
||||
#include "flightstatus.h"
|
||||
#include "manualcontrol.h" // Just to get a macro
|
||||
@ -129,6 +130,9 @@ int32_t StabilizationInitialize()
|
||||
ActuatorDesiredInitialize();
|
||||
#ifdef DIAG_RATEDESIRED
|
||||
RateDesiredInitialize();
|
||||
#endif
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateInitialize();
|
||||
#endif
|
||||
// Code required for relay tuning
|
||||
sin_lookup_initalize();
|
||||
@ -156,6 +160,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
GyroStateData gyroStateData;
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateData airspeedState;
|
||||
#endif
|
||||
|
||||
SettingsUpdatedCb((UAVObjEvent *)NULL);
|
||||
|
||||
// Main task loop
|
||||
@ -183,6 +191,26 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
#ifdef DIAG_RATEDESIRED
|
||||
RateDesiredGet(&rateDesired);
|
||||
#endif
|
||||
#ifdef REVOLUTION
|
||||
float speedScaleFactor;
|
||||
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
|
||||
AirspeedStateGet(&airspeedState);
|
||||
if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
|
||||
// feature has been turned off
|
||||
speedScaleFactor = 1.0f;
|
||||
} else {
|
||||
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
|
||||
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
|
||||
if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) {
|
||||
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN];
|
||||
}
|
||||
if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) {
|
||||
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX];
|
||||
}
|
||||
}
|
||||
#else
|
||||
const float speedScaleFactor = 1.0f;
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_QUATERNION_STABILIZATION)
|
||||
// Quaternion calculation of error in each axis. Uses more memory.
|
||||
@ -262,7 +290,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
@ -278,7 +306,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
@ -304,7 +332,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
// Compute desired rate as input biased towards leveling
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
@ -328,7 +356,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate.data[i]);
|
||||
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
|
@ -32,19 +32,16 @@
|
||||
|
||||
#include "inc/stateestimation.h"
|
||||
|
||||
#include <revosettings.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
#define STACK_REQUIRED 64
|
||||
|
||||
// low pass filter configuration to calculate offset
|
||||
// of barometric altitude sensor
|
||||
// reasoning: updates at: 10 Hz, tau= 300 s settle time
|
||||
// exp(-(1/f) / tau ) ~=~ 0.9997
|
||||
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
|
||||
|
||||
// Private types
|
||||
struct data {
|
||||
float baroOffset;
|
||||
float baroGPSOffsetCorrectionAlpha;
|
||||
float baroAlt;
|
||||
int16_t first_run;
|
||||
};
|
||||
@ -71,6 +68,10 @@ static int32_t init(stateFilter *self)
|
||||
|
||||
this->baroOffset = 0.0f;
|
||||
this->first_run = 100;
|
||||
|
||||
RevoSettingsInitialize();
|
||||
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -90,9 +91,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
|
||||
// Track barometric altitude offset with a low pass filter
|
||||
// based on GPS altitude if available
|
||||
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
|
||||
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
|
||||
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
|
||||
* (this->baroAlt + state->pos[2]);
|
||||
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
|
||||
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
|
||||
}
|
||||
// calculate bias corrected altitude
|
||||
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
|
||||
|
@ -55,6 +55,8 @@ struct data {
|
||||
bool useMag;
|
||||
float currentAccel[3];
|
||||
float currentMag[3];
|
||||
float accels_filtered[3];
|
||||
float grot_filtered[3];
|
||||
float gyroBias[3];
|
||||
bool accelUpdated;
|
||||
bool magUpdated;
|
||||
@ -266,7 +268,12 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
||||
RPY2Quaternion(&attitudeState.Roll, attitude);
|
||||
|
||||
this->first_run = 0;
|
||||
|
||||
this->accels_filtered[0] = 0.0f;
|
||||
this->accels_filtered[1] = 0.0f;
|
||||
this->accels_filtered[2] = 0.0f;
|
||||
this->grot_filtered[0] = 0.0f;
|
||||
this->grot_filtered[1] = 0.0f;
|
||||
this->grot_filtered[2] = 0.0f;
|
||||
this->timeval = PIOS_DELAY_GetRaw();
|
||||
this->starttime = this->timeval;
|
||||
|
||||
@ -316,9 +323,8 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
||||
// Get the current attitude estimate
|
||||
quat_copy(&attitudeState.q1, attitude);
|
||||
|
||||
float accels_filtered[3];
|
||||
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
|
||||
apply_accel_filter(this, accel, accels_filtered);
|
||||
apply_accel_filter(this, accel, this->accels_filtered);
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
float grot[3];
|
||||
@ -326,13 +332,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
||||
grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
|
||||
grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
|
||||
|
||||
float grot_filtered[3];
|
||||
float accel_err[3];
|
||||
apply_accel_filter(this, grot, grot_filtered);
|
||||
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
|
||||
apply_accel_filter(this, grot, this->grot_filtered);
|
||||
|
||||
CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
|
||||
|
||||
// Account for accel magnitude
|
||||
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
|
||||
float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
|
||||
if (accel_mag < 1.0e-3f) {
|
||||
return 2; // safety feature copied from CC
|
||||
}
|
||||
@ -340,7 +346,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
||||
// Account for filtered gravity vector magnitude
|
||||
float grot_mag;
|
||||
if (this->accel_filter_enabled) {
|
||||
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
|
||||
grot_mag = sqrtf(this->grot_filtered[0] * this->grot_filtered[0] + this->grot_filtered[1] * this->grot_filtered[1] + this->grot_filtered[2] * this->grot_filtered[2]);
|
||||
} else {
|
||||
grot_mag = 1.0f;
|
||||
}
|
||||
|
@ -253,51 +253,58 @@ static void systemTask(__attribute__((unused)) void *parameters)
|
||||
int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
|
||||
|
||||
#if defined(PIOS_INCLUDE_RFM22B)
|
||||
|
||||
// Update the OPLinkStatus UAVO
|
||||
OPLinkStatusData oplinkStatus;
|
||||
OPLinkStatusGet(&oplinkStatus);
|
||||
|
||||
// Get the other device stats.
|
||||
PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
|
||||
if (pios_rfm22b_id) {
|
||||
// Get the other device stats.
|
||||
PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
|
||||
|
||||
// Get the stats from the radio device
|
||||
struct rfm22b_stats radio_stats;
|
||||
PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
|
||||
// Get the stats from the radio device
|
||||
struct rfm22b_stats radio_stats;
|
||||
PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
|
||||
|
||||
// Update the OPLInk status
|
||||
static bool first_time = true;
|
||||
static uint16_t prev_tx_count = 0;
|
||||
static uint16_t prev_rx_count = 0;
|
||||
oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
|
||||
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
||||
oplinkStatus.RxGood = radio_stats.rx_good;
|
||||
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
|
||||
oplinkStatus.RxErrors = radio_stats.rx_error;
|
||||
oplinkStatus.RxMissed = radio_stats.rx_missed;
|
||||
oplinkStatus.RxFailure = radio_stats.rx_failure;
|
||||
oplinkStatus.TxDropped = radio_stats.tx_dropped;
|
||||
oplinkStatus.TxResent = radio_stats.tx_resent;
|
||||
oplinkStatus.TxFailure = radio_stats.tx_failure;
|
||||
oplinkStatus.Resets = radio_stats.resets;
|
||||
oplinkStatus.Timeouts = radio_stats.timeouts;
|
||||
oplinkStatus.RSSI = radio_stats.rssi;
|
||||
oplinkStatus.LinkQuality = radio_stats.link_quality;
|
||||
if (first_time) {
|
||||
first_time = false;
|
||||
// Update the OPLInk status
|
||||
static bool first_time = true;
|
||||
static uint16_t prev_tx_count = 0;
|
||||
static uint16_t prev_rx_count = 0;
|
||||
oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
|
||||
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
||||
oplinkStatus.RxGood = radio_stats.rx_good;
|
||||
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
|
||||
oplinkStatus.RxErrors = radio_stats.rx_error;
|
||||
oplinkStatus.RxMissed = radio_stats.rx_missed;
|
||||
oplinkStatus.RxFailure = radio_stats.rx_failure;
|
||||
oplinkStatus.TxDropped = radio_stats.tx_dropped;
|
||||
oplinkStatus.TxResent = radio_stats.tx_resent;
|
||||
oplinkStatus.TxFailure = radio_stats.tx_failure;
|
||||
oplinkStatus.Resets = radio_stats.resets;
|
||||
oplinkStatus.Timeouts = radio_stats.timeouts;
|
||||
oplinkStatus.RSSI = radio_stats.rssi;
|
||||
oplinkStatus.LinkQuality = radio_stats.link_quality;
|
||||
if (first_time) {
|
||||
first_time = false;
|
||||
} else {
|
||||
uint16_t tx_count = radio_stats.tx_byte_count;
|
||||
uint16_t rx_count = radio_stats.rx_byte_count;
|
||||
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
|
||||
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
|
||||
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
prev_tx_count = tx_count;
|
||||
prev_rx_count = rx_count;
|
||||
}
|
||||
oplinkStatus.TXSeq = radio_stats.tx_seq;
|
||||
oplinkStatus.RXSeq = radio_stats.rx_seq;
|
||||
|
||||
oplinkStatus.LinkState = radio_stats.link_state;
|
||||
} else {
|
||||
uint16_t tx_count = radio_stats.tx_byte_count;
|
||||
uint16_t rx_count = radio_stats.rx_byte_count;
|
||||
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
|
||||
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
|
||||
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
||||
prev_tx_count = tx_count;
|
||||
prev_rx_count = rx_count;
|
||||
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
|
||||
}
|
||||
oplinkStatus.TXSeq = radio_stats.tx_seq;
|
||||
oplinkStatus.RXSeq = radio_stats.rx_seq;
|
||||
oplinkStatus.LinkState = radio_stats.link_state;
|
||||
OPLinkStatusSet(&oplinkStatus);
|
||||
|
||||
#endif /* if defined(PIOS_INCLUDE_RFM22B) */
|
||||
|
||||
if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
|
||||
|
81
flight/pios/common/pios_led.c
Normal file
81
flight/pios/common/pios_led.c
Normal file
@ -0,0 +1,81 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_LED LED Functions
|
||||
* @brief STM32 Hardware LED handling code
|
||||
* @{
|
||||
*
|
||||
* @file pios_led.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief LED functions, init, toggle, on & off.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#ifdef PIOS_INCLUDE_LED
|
||||
|
||||
#include <pios_gpio_priv.h>
|
||||
#include <pios_gpio.h>
|
||||
|
||||
static uint32_t pios_led_gpios_id;
|
||||
|
||||
/**
|
||||
* Initialises all the LED's
|
||||
*/
|
||||
int32_t PIOS_LED_Init(const struct pios_gpio_cfg *cfg)
|
||||
{
|
||||
PIOS_Assert(cfg);
|
||||
return PIOS_GPIO_Init(&pios_led_gpios_id, cfg);
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn on LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_On(uint32_t led_id)
|
||||
{
|
||||
PIOS_GPIO_On(pios_led_gpios_id, led_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn off LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Off(uint32_t led_id)
|
||||
{
|
||||
PIOS_GPIO_Off(pios_led_gpios_id, led_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggle LED on/off
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Toggle(uint32_t led_id)
|
||||
{
|
||||
PIOS_GPIO_Toggle(pios_led_gpios_id, led_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -32,11 +32,9 @@
|
||||
#define PIOS_GPIO_H
|
||||
|
||||
/* Public Functions */
|
||||
extern void PIOS_GPIO_Init(void);
|
||||
extern void PIOS_GPIO_Enable(uint8_t Pin);
|
||||
extern void PIOS_GPIO_On(uint8_t Pin);
|
||||
extern void PIOS_GPIO_Off(uint8_t Pin);
|
||||
extern void PIOS_GPIO_Toggle(uint8_t Pin);
|
||||
extern void PIOS_GPIO_On(uint32_t gpios_dev_id, uint8_t gpio_id);
|
||||
extern void PIOS_GPIO_Off(uint32_t gpios_dev_id, uint8_t gpio_id);
|
||||
extern void PIOS_GPIO_Toggle(uint32_t gpios_dev_id, uint8_t gpio_id);
|
||||
|
||||
#endif /* PIOS_GPIO_H */
|
||||
|
||||
|
55
flight/pios/inc/pios_gpio_priv.h
Normal file
55
flight/pios/inc/pios_gpio_priv.h
Normal file
@ -0,0 +1,55 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_GPIO GPIO Functions
|
||||
* @brief PIOS interface for GPIOss
|
||||
* @{
|
||||
*
|
||||
* @file pios_gpio_priv.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||
* @brief GPIO private definitions.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_GPIO_PRIV_H
|
||||
#define PIOS_GPIO_PRIV_H
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_stm32.h>
|
||||
|
||||
struct pios_gpio {
|
||||
struct stm32_gpio pin;
|
||||
uint32_t remap;
|
||||
bool active_low;
|
||||
};
|
||||
|
||||
struct pios_gpio_cfg {
|
||||
const struct pios_gpio *gpios;
|
||||
uint8_t num_gpios;
|
||||
};
|
||||
|
||||
extern int32_t PIOS_GPIO_Init(uint32_t *gpios_dev_id, const struct pios_gpio_cfg *cfg);
|
||||
|
||||
#endif /* PIOS_GPIO_PRIV_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -31,21 +31,9 @@
|
||||
#ifndef PIOS_LED_PRIV_H
|
||||
#define PIOS_LED_PRIV_H
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_stm32.h>
|
||||
#include <pios_gpio_priv.h>
|
||||
|
||||
struct pios_led {
|
||||
struct stm32_gpio pin;
|
||||
uint32_t remap;
|
||||
bool active_high;
|
||||
};
|
||||
|
||||
struct pios_led_cfg {
|
||||
const struct pios_led *leds;
|
||||
uint8_t num_leds;
|
||||
};
|
||||
|
||||
extern int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg);
|
||||
extern int32_t PIOS_LED_Init(const struct pios_gpio_cfg *cfg);
|
||||
|
||||
#endif /* PIOS_LED_PRIV_H */
|
||||
|
||||
|
@ -81,6 +81,8 @@
|
||||
#include "semphr.h"
|
||||
#endif
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <pios_architecture.h>
|
||||
|
||||
#ifdef PIOS_INCLUDE_TASK_MONITOR
|
||||
|
@ -2,12 +2,12 @@
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @defgroup PIOS_GPIO GPIO Functions
|
||||
* @brief GPIO hardware code for STM32
|
||||
* @addtogroup PIOS_GPIO GPIO Functions
|
||||
* @brief STM32 Hardware GPIO handling code
|
||||
* @{
|
||||
*
|
||||
* @file pios_gpio.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||
* @brief GPIO functions, init, toggle, on & off.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -32,65 +32,125 @@
|
||||
|
||||
#ifdef PIOS_INCLUDE_GPIO
|
||||
|
||||
/* Private Function Prototypes */
|
||||
|
||||
/* Local Variables */
|
||||
static GPIO_TypeDef *GPIO_PORT[PIOS_GPIO_NUM] = PIOS_GPIO_PORTS;
|
||||
static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS;
|
||||
static const uint32_t GPIO_CLK[PIOS_GPIO_NUM] = PIOS_GPIO_CLKS;
|
||||
#include <pios_gpio_priv.h>
|
||||
|
||||
/**
|
||||
* Initialises all the GPIO's
|
||||
*/
|
||||
void PIOS_GPIO_Init(void)
|
||||
int32_t PIOS_GPIO_Init(uint32_t *gpios_dev_id, const struct pios_gpio_cfg *cfg)
|
||||
{
|
||||
/* Do nothing */
|
||||
PIOS_Assert(cfg);
|
||||
*gpios_dev_id = (uint32_t)cfg;
|
||||
|
||||
for (uint8_t i = 0; i < cfg->num_gpios; i++) {
|
||||
const struct pios_gpio *gpio = &(cfg->gpios[i]);
|
||||
|
||||
/* Enable the peripheral clock for the GPIO */
|
||||
switch ((uint32_t)gpio->pin.gpio) {
|
||||
case (uint32_t)GPIOA:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOB:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOC:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
if (gpio->remap) {
|
||||
GPIO_PinRemapConfig(gpio->remap, ENABLE);
|
||||
}
|
||||
|
||||
GPIO_Init(gpio->pin.gpio, &gpio->pin.init);
|
||||
|
||||
PIOS_GPIO_Off(*gpios_dev_id, i);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enable a GPIO Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Turn on GPIO
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_Enable(uint8_t Pin)
|
||||
void PIOS_GPIO_On(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
// RCC_APB2PeriphClockCmd(GPIO_CLK[Pin], ENABLE);
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin];
|
||||
GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure);
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
/* GPIO's Off */
|
||||
GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin];
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (gpio->active_low) {
|
||||
GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn on Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Turn off GPIO
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_On(uint8_t Pin)
|
||||
void PIOS_GPIO_Off(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
GPIO_PORT[Pin]->BRR = GPIO_PIN[Pin];
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (gpio->active_low) {
|
||||
GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn off Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Toggle GPIO on/off
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_Off(uint8_t Pin)
|
||||
void PIOS_GPIO_Toggle(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin];
|
||||
}
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
/**
|
||||
* Toggle Pin on/off
|
||||
* \param[in] Pin Pin Number
|
||||
*/
|
||||
void PIOS_GPIO_Toggle(uint8_t Pin)
|
||||
{
|
||||
GPIO_PORT[Pin]->ODR ^= GPIO_PIN[Pin];
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (GPIO_ReadOutputDataBit(gpio->pin.gpio, gpio->pin.init.GPIO_Pin) == Bit_SET) {
|
||||
if (gpio->active_low) {
|
||||
PIOS_GPIO_On(gpios_dev_id, gpio_id);
|
||||
} else {
|
||||
PIOS_GPIO_Off(gpios_dev_id, gpio_id);
|
||||
}
|
||||
} else {
|
||||
if (gpio->active_low) {
|
||||
PIOS_GPIO_Off(gpios_dev_id, gpio_id);
|
||||
} else {
|
||||
PIOS_GPIO_On(gpios_dev_id, gpio_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_GPIO */
|
||||
|
@ -1,159 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_LED LED Functions
|
||||
* @brief STM32 Hardware LED handling code
|
||||
* @{
|
||||
*
|
||||
* @file pios_led.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief LED functions, init, toggle, on & off.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#ifdef PIOS_INCLUDE_LED
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
|
||||
static const struct pios_led_cfg *led_cfg;
|
||||
|
||||
/**
|
||||
* Initialises all the LED's
|
||||
*/
|
||||
int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg)
|
||||
{
|
||||
PIOS_Assert(cfg);
|
||||
|
||||
/* Store away the config in a global used by API functions */
|
||||
led_cfg = cfg;
|
||||
|
||||
for (uint8_t i = 0; i < cfg->num_leds; i++) {
|
||||
const struct pios_led *led = &(cfg->leds[i]);
|
||||
|
||||
/* Enable the peripheral clock for the GPIO */
|
||||
switch ((uint32_t)led->pin.gpio) {
|
||||
case (uint32_t)GPIOA:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOB:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOC:
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
if (led->remap) {
|
||||
GPIO_PinRemapConfig(led->remap, ENABLE);
|
||||
}
|
||||
|
||||
GPIO_Init(led->pin.gpio, &led->pin.init);
|
||||
|
||||
PIOS_LED_Off(i);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn on LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_On(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (led->active_high) {
|
||||
GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn off LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Off(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (led->active_high) {
|
||||
GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggle LED on/off
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Toggle(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) {
|
||||
if (led->active_high) {
|
||||
PIOS_LED_Off(led_id);
|
||||
} else {
|
||||
PIOS_LED_On(led_id);
|
||||
}
|
||||
} else {
|
||||
if (led->active_high) {
|
||||
PIOS_LED_On(led_id);
|
||||
} else {
|
||||
PIOS_LED_Off(led_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -2,8 +2,8 @@
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @defgroup PIOS_GPIO GPIO Functions
|
||||
* @brief GPIO hardware code for STM32F4xx
|
||||
* @addtogroup PIOS_GPIO GPIO Functions
|
||||
* @brief STM32 Hardware GPIO handling code
|
||||
* @{
|
||||
*
|
||||
* @file pios_gpio.c
|
||||
@ -32,64 +32,143 @@
|
||||
|
||||
#ifdef PIOS_INCLUDE_GPIO
|
||||
|
||||
/* Private Function Prototypes */
|
||||
|
||||
/* Local Variables */
|
||||
static GPIO_TypeDef *GPIO_PORT[PIOS_GPIO_NUM] = PIOS_GPIO_PORTS;
|
||||
static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS;
|
||||
#include <pios_gpio_priv.h>
|
||||
|
||||
/**
|
||||
* Initialises all the GPIO's
|
||||
*/
|
||||
void PIOS_GPIO_Init(void)
|
||||
int32_t PIOS_GPIO_Init(uint32_t *gpios_dev_id, const struct pios_gpio_cfg *cfg)
|
||||
{
|
||||
/* Do nothing */
|
||||
PIOS_Assert(cfg);
|
||||
*gpios_dev_id = (uint32_t)cfg;
|
||||
|
||||
for (uint8_t i = 0; i < cfg->num_gpios; i++) {
|
||||
const struct pios_gpio *gpio = &(cfg->gpios[i]);
|
||||
|
||||
/* Enable the peripheral clock for the GPIO */
|
||||
switch ((uint32_t)gpio->pin.gpio) {
|
||||
case (uint32_t)GPIOA:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOB:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOC:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOD:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOE:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOF:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOG:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOH:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOI:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
if (gpio->remap) {
|
||||
GPIO_PinAFConfig(gpio->pin.gpio, gpio->pin.init.GPIO_Pin, gpio->remap);
|
||||
}
|
||||
|
||||
GPIO_Init(gpio->pin.gpio, &gpio->pin.init);
|
||||
|
||||
PIOS_GPIO_Off(*gpios_dev_id, i);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enable a GPIO Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Turn on GPIO
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_Enable(uint8_t Pin)
|
||||
void PIOS_GPIO_On(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin];
|
||||
GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure);
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
/* GPIO's Off */
|
||||
PIOS_GPIO_Off(Pin);
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (gpio->active_low) {
|
||||
GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn on Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Turn off GPIO
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_On(uint8_t Pin)
|
||||
void PIOS_GPIO_Off(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
GPIO_ResetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]);
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (gpio->active_low) {
|
||||
GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn off Pin
|
||||
* \param[in] Pin Pin Number
|
||||
* Toggle GPIO on/off
|
||||
* \param[in] GPIO GPIO id
|
||||
*/
|
||||
void PIOS_GPIO_Off(uint8_t Pin)
|
||||
void PIOS_GPIO_Toggle(uint32_t gpios_dev_id, uint8_t gpio_id)
|
||||
{
|
||||
GPIO_SetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]);
|
||||
}
|
||||
const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id;
|
||||
|
||||
/**
|
||||
* Toggle Pin on/off
|
||||
* \param[in] Pin Pin Number
|
||||
*/
|
||||
void PIOS_GPIO_Toggle(uint8_t Pin)
|
||||
{
|
||||
GPIO_ToggleBits(GPIO_PORT[Pin], GPIO_PIN[Pin]);
|
||||
PIOS_Assert(gpio_cfg);
|
||||
|
||||
if (gpio_id >= gpio_cfg->num_gpios) {
|
||||
/* GPIO index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]);
|
||||
|
||||
if (GPIO_ReadOutputDataBit(gpio->pin.gpio, gpio->pin.init.GPIO_Pin) == Bit_SET) {
|
||||
if (gpio->active_low) {
|
||||
PIOS_GPIO_On(gpios_dev_id, gpio_id);
|
||||
} else {
|
||||
PIOS_GPIO_Off(gpios_dev_id, gpio_id);
|
||||
}
|
||||
} else {
|
||||
if (gpio->active_low) {
|
||||
PIOS_GPIO_Off(gpios_dev_id, gpio_id);
|
||||
} else {
|
||||
PIOS_GPIO_On(gpios_dev_id, gpio_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_GPIO */
|
||||
|
@ -1,177 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_LED LED Functions
|
||||
* @brief STM32 Hardware LED handling code
|
||||
* @{
|
||||
*
|
||||
* @file pios_led.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief LED functions, init, toggle, on & off.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#ifdef PIOS_INCLUDE_LED
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
|
||||
static const struct pios_led_cfg *led_cfg;
|
||||
|
||||
/**
|
||||
* Initialises all the LED's
|
||||
*/
|
||||
int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg)
|
||||
{
|
||||
PIOS_Assert(cfg);
|
||||
|
||||
/* Store away the config in a global used by API functions */
|
||||
led_cfg = cfg;
|
||||
|
||||
for (uint8_t i = 0; i < cfg->num_leds; i++) {
|
||||
const struct pios_led *led = &(cfg->leds[i]);
|
||||
|
||||
/* Enable the peripheral clock for the GPIO */
|
||||
switch ((uint32_t)led->pin.gpio) {
|
||||
case (uint32_t)GPIOA:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOB:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOC:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOD:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOE:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOF:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOG:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOH:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE);
|
||||
break;
|
||||
case (uint32_t)GPIOI:
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
if (led->remap) {
|
||||
GPIO_PinAFConfig(led->pin.gpio, led->pin.init.GPIO_Pin, led->remap);
|
||||
}
|
||||
|
||||
GPIO_Init(led->pin.gpio, &led->pin.init);
|
||||
|
||||
PIOS_LED_Off(i);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn on LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_On(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (led->active_high) {
|
||||
GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Turn off LED
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Off(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (led->active_high) {
|
||||
GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
} else {
|
||||
GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggle LED on/off
|
||||
* \param[in] LED LED id
|
||||
*/
|
||||
void PIOS_LED_Toggle(uint32_t led_id)
|
||||
{
|
||||
PIOS_Assert(led_cfg);
|
||||
|
||||
if (led_id >= led_cfg->num_leds) {
|
||||
/* LED index out of range */
|
||||
return;
|
||||
}
|
||||
|
||||
const struct pios_led *led = &(led_cfg->leds[led_id]);
|
||||
|
||||
if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) {
|
||||
if (led->active_high) {
|
||||
PIOS_LED_Off(led_id);
|
||||
} else {
|
||||
PIOS_LED_On(led_id);
|
||||
}
|
||||
} else {
|
||||
if (led->active_high) {
|
||||
PIOS_LED_On(led_id);
|
||||
} else {
|
||||
PIOS_LED_Off(led_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -31,7 +31,7 @@
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds_cc[] = {
|
||||
static const struct pios_gpio pios_leds_cc[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
@ -41,15 +41,16 @@ static const struct pios_led pios_leds_cc[] = {
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg_cc = {
|
||||
.leds = pios_leds_cc,
|
||||
.num_leds = NELEMENTS(pios_leds_cc),
|
||||
static const struct pios_gpio_cfg pios_led_cfg_cc = {
|
||||
.gpios = pios_leds_cc,
|
||||
.num_gpios = NELEMENTS(pios_leds_cc),
|
||||
};
|
||||
|
||||
static const struct pios_led pios_leds_cc3d[] = {
|
||||
static const struct pios_gpio pios_leds_cc3d[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
@ -60,15 +61,16 @@ static const struct pios_led pios_leds_cc3d[] = {
|
||||
},
|
||||
},
|
||||
.remap = GPIO_Remap_SWJ_JTAGDisable,
|
||||
.active_low = true
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg_cc3d = {
|
||||
.leds = pios_leds_cc3d,
|
||||
.num_leds = NELEMENTS(pios_leds_cc3d),
|
||||
static const struct pios_gpio_cfg pios_led_cfg_cc3d = {
|
||||
.gpios = pios_leds_cc3d,
|
||||
.num_gpios = NELEMENTS(pios_leds_cc3d),
|
||||
};
|
||||
|
||||
const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
|
||||
{
|
||||
switch (board_revision) {
|
||||
case BOARD_REVISION_CC: return &pios_led_cfg_cc;
|
||||
|
@ -59,13 +59,10 @@ void PIOS_Board_Init(void)
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
PIOS_Assert(led_cfg);
|
||||
PIOS_LED_Init(led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
@ -145,7 +145,7 @@ void PIOS_Board_Init(void)
|
||||
const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
PIOS_Assert(led_cfg);
|
||||
PIOS_LED_Init(led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
@ -865,8 +865,6 @@ void PIOS_Board_Init(void)
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
/* Make sure we have at least one telemetry link configured or else fail initialization */
|
||||
PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
|
||||
}
|
||||
|
@ -27,7 +27,7 @@
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds[] = {
|
||||
static const struct pios_gpio pios_leds[] = {
|
||||
[PIOS_LED_USB] = {
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
@ -37,6 +37,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_LINK] = {
|
||||
.pin = {
|
||||
@ -47,6 +48,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_RX] = {
|
||||
.pin = {
|
||||
@ -57,6 +59,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_TX] = {
|
||||
.pin = {
|
||||
@ -67,6 +70,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
|
||||
[PIOS_LED_D1] = {
|
||||
@ -112,12 +116,12 @@ static const struct pios_led pios_leds[] = {
|
||||
#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg = {
|
||||
.leds = pios_leds,
|
||||
.num_leds = NELEMENTS(pios_leds),
|
||||
static const struct pios_gpio_cfg pios_led_cfg = {
|
||||
.gpios = pios_leds,
|
||||
.num_gpios = NELEMENTS(pios_leds),
|
||||
};
|
||||
|
||||
const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_led_cfg;
|
||||
}
|
||||
|
@ -59,9 +59,6 @@ void PIOS_Board_Init(void)
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
@ -485,7 +485,6 @@ void PIOS_Board_Init(void)
|
||||
#ifdef PIOS_INCLUDE_ADC
|
||||
PIOS_ADC_Init();
|
||||
#endif
|
||||
PIOS_GPIO_Init();
|
||||
}
|
||||
|
||||
static void PIOS_Board_PPM_callback(const int16_t *channels)
|
||||
|
@ -30,7 +30,7 @@
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds[] = {
|
||||
static const struct pios_gpio pios_leds[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
@ -42,6 +42,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_ALARM] = {
|
||||
.pin = {
|
||||
@ -54,15 +55,16 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg = {
|
||||
.leds = pios_leds,
|
||||
.num_leds = NELEMENTS(pios_leds),
|
||||
static const struct pios_gpio_cfg pios_led_cfg = {
|
||||
.gpios = pios_leds,
|
||||
.num_gpios = NELEMENTS(pios_leds),
|
||||
};
|
||||
|
||||
const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_led_cfg;
|
||||
}
|
||||
|
@ -35,6 +35,7 @@
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_COM_MSG
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
@ -61,7 +61,7 @@
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_SPI
|
||||
/* #define PIOS_INCLUDE_GPIO */
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_WDG
|
||||
|
||||
|
@ -28,7 +28,7 @@
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds[] = {
|
||||
static const struct pios_gpio pios_leds[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
@ -40,6 +40,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_ALARM] = {
|
||||
.pin = {
|
||||
@ -52,6 +53,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
|
||||
[PIOS_LED_D1] = {
|
||||
@ -105,12 +107,12 @@ static const struct pios_led pios_leds[] = {
|
||||
#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg = {
|
||||
.leds = pios_leds,
|
||||
.num_leds = NELEMENTS(pios_leds),
|
||||
static const struct pios_gpio_cfg pios_led_cfg = {
|
||||
.gpios = pios_leds,
|
||||
.num_gpios = NELEMENTS(pios_leds),
|
||||
};
|
||||
|
||||
static const struct pios_led pios_leds_v2[] = {
|
||||
static const struct pios_gpio pios_leds_v2[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
@ -122,6 +124,7 @@ static const struct pios_led pios_leds_v2[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_ALARM] = {
|
||||
.pin = {
|
||||
@ -134,6 +137,7 @@ static const struct pios_led pios_leds_v2[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
|
||||
[PIOS_LED_D1] = {
|
||||
@ -187,12 +191,12 @@ static const struct pios_led pios_leds_v2[] = {
|
||||
#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_v2_cfg = {
|
||||
.leds = pios_leds_v2,
|
||||
.num_leds = NELEMENTS(pios_leds_v2),
|
||||
static const struct pios_gpio_cfg pios_led_v2_cfg = {
|
||||
.gpios = pios_leds_v2,
|
||||
.num_gpios = NELEMENTS(pios_leds_v2),
|
||||
};
|
||||
|
||||
const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
|
||||
{
|
||||
switch (board_revision) {
|
||||
case 2:
|
||||
|
@ -35,6 +35,7 @@
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_COM_MSG
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
@ -51,7 +51,7 @@ void PIOS_Board_Init()
|
||||
const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
PIOS_Assert(led_cfg);
|
||||
PIOS_LED_Init(led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
@ -61,7 +61,7 @@
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_SPI
|
||||
/* #define PIOS_INCLUDE_GPIO */
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_WDG
|
||||
|
||||
|
@ -346,7 +346,7 @@ void PIOS_Board_Init(void)
|
||||
const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
PIOS_Assert(led_cfg);
|
||||
PIOS_LED_Init(led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
@ -28,7 +28,7 @@
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds[] = {
|
||||
static const struct pios_gpio pios_leds[] = {
|
||||
[PIOS_LED_HEARTBEAT] = {
|
||||
.pin = {
|
||||
.gpio = GPIOE,
|
||||
@ -40,6 +40,7 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
[PIOS_LED_ALARM] = {
|
||||
.pin = {
|
||||
@ -52,15 +53,16 @@ static const struct pios_led pios_leds[] = {
|
||||
.GPIO_PuPd = GPIO_PuPd_UP
|
||||
},
|
||||
},
|
||||
.active_low = true
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg = {
|
||||
.leds = pios_leds,
|
||||
.num_leds = NELEMENTS(pios_leds),
|
||||
static const struct pios_gpio_cfg pios_led_cfg = {
|
||||
.gpios = pios_leds,
|
||||
.num_gpios = NELEMENTS(pios_leds),
|
||||
};
|
||||
|
||||
const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
|
||||
{
|
||||
return &pios_led_cfg;
|
||||
}
|
||||
|
@ -35,6 +35,7 @@
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_COM_MSG
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
@ -61,7 +61,7 @@
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_SPI
|
||||
/* #define PIOS_INCLUDE_GPIO */
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_WDG
|
||||
|
||||
|
@ -39,6 +39,7 @@ HWDEFSINC = ../../boards/$(BOARD_NAME)
|
||||
# Use file-extension c for "c-only"-files
|
||||
SRC += $(OPSYSTEM)/main.c
|
||||
SRC += $(OPSYSTEM)/pios_board.c
|
||||
SRC += $(PIOSCOMMON)/pios_led.c
|
||||
## PIOS Hardware
|
||||
ifeq ($(MCU),cortex-m3)
|
||||
include $(PIOS)/stm32f10x/library.mk
|
||||
|
@ -35,6 +35,7 @@
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
|
||||
|
||||
|
@ -41,13 +41,8 @@ void PIOS_Board_Init(void)
|
||||
|
||||
/* LEDs */
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
|
||||
PIOS_Assert(led_cfg);
|
||||
PIOS_LED_Init(led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
#if defined(PIOS_INCLUDE_GPIO)
|
||||
PIOS_GPIO_Init();
|
||||
#endif /* PIOS_INCLUDE_GPIO */
|
||||
}
|
||||
|
@ -1,4 +1,4 @@
|
||||
html>
|
||||
<html>
|
||||
<head>
|
||||
<title></title>
|
||||
<meta content="">
|
||||
|
@ -86,6 +86,7 @@ SRC += $(PIOSCOMMON)/pios_rfm22b.c
|
||||
SRC += $(PIOSCOMMON)/pios_rfm22b_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_sbus.c
|
||||
SRC += $(PIOSCOMMON)/pios_sdcard.c
|
||||
SRC += $(PIOSCOMMON)/pios_led.c
|
||||
|
||||
## PIOS USB related files
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c
|
||||
|
@ -55,6 +55,7 @@ SRC += $(PIOSCOMMON)/pios_com_msg.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_util.c
|
||||
SRC += $(PIOSCOMMON)/pios_led.c
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/op_dfu.c
|
||||
|
@ -11,8 +11,8 @@
|
||||
<field name="AngleOfAttack" units="deg" type="float" elements="1"/>
|
||||
<field name="AngleOfSlip" units="deg" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readonly"/>
|
||||
<telemetrygcs acked="false" updatemode="periodic" period="1000"/>
|
||||
<telemetryflight acked="false" updatemode="manual" period="50000"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="manual" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
@ -2,6 +2,12 @@
|
||||
<object name="RevoSettings" singleinstance="true" settings="true" category="State">
|
||||
<description>Settings for the revo to control the algorithm and what is updated</description>
|
||||
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor" defaultvalue="Complementary"/>
|
||||
|
||||
<!-- Low pass filter configuration to calculate offset of barometric altitude sensor.
|
||||
Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
|
||||
Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
|
||||
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
@ -37,6 +37,9 @@
|
||||
|
||||
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
|
||||
|
||||
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user