rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into next

Browse Source
This commit is contained in:
Philippe Renon 2014-05-11 14:56:56 +02:00
commit 3a02149d1d
89 changed files with 8200 additions and 1705 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Makefile
View File

@ -197,7 +197,7 @@ export OPUAVSYNTHDIR := $(BUILD_DIR)/uavobject-synthetics/flight
export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
# Define supported board lists
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare
# Short names of each board (used to display board name in parallel builds)
coptercontrol_short := 'cc '
@ -206,6 +206,7 @@ revolution_short := 'revo'
osd_short := 'osd '
revoproto_short := 'revp'
simposix_short := 'posx'
discoveryf4bare_short := 'df4b'
# SimPosix only builds on Linux so drop it from the list for
# all other platforms.
@ -697,7 +698,7 @@ endif
##############################
# Firmware files to package
PACKAGE_FW_TARGETS := $(filter-out fw_simposix, $(FW_TARGETS))
PACKAGE_FW_TARGETS := $(filter-out fw_simposix fw_discoveryf4bare, $(FW_TARGETS))
PACKAGE_ELF_TARGETS := $(filter fw_simposix, $(FW_TARGETS))
# Rules to generate GCS resources used to embed firmware binaries into the GCS.

31
flight/libraries/alarms.c
View File

@ -82,7 +82,7 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
// Read alarm and update its severity only if it was changed
SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
@ -121,7 +121,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
@ -254,6 +254,33 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* Get the highest alarm severity
* @return
*/
SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
{
SystemAlarmsAlarmData alarmsData;
SystemAlarmsAlarmOptions highest = SYSTEMALARMS_ALARM_UNINITIALISED;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsAlarmGet(&alarmsData);
// Go through alarms and find the highest severity
uint32_t n = 0;
while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) {
highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n];
}
n++;
}
xSemaphoreGiveRecursive(lock);
return highest;
}
/**
* @}

2
flight/libraries/inc/alarms.h
View File

@ -42,9 +42,11 @@ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
void AlarmsDefaultAll();
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
void AlarmsClearAll();
int32_t AlarmsHasWarnings();
int32_t AlarmsHasErrors();
int32_t AlarmsHasCritical();
SystemAlarmsAlarmOptions AlarmsGetHighestSeverity();
#endif // ALARMS_H

38
flight/libraries/inc/notification.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
*
* @file notification.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief notification library
* --
* @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 NOTIFICATION_H
#define NOTIFICATION_H
// period of each blink phase
#define LED_BLINK_PERIOD_MS 200
// update the status snapshot used by notifcations
void NotificationUpdateStatus();
// run the led notifications
void NotificationOnboardLedsRun();
#endif /* NOTIFICATION_H */

296
flight/libraries/notification.c Normal file
View File

@ -0,0 +1,296 @@
/**
******************************************************************************
*
* @file notification.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief notification library.
* --
* @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 "inc/notification.h"
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <systemalarms.h>
#include <flightstatus.h>
#include <pios_notify.h>
#ifdef PIOS_LED_ALARM
#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM)
#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM)
#else
#define ALARM_LED_ON()
#define ALARM_LED_OFF()
#endif
#ifdef PIOS_LED_HEARTBEAT
#define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT)
#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT)
#else
#define HEARTBEAT_LED_ON()
#define HEARTBEAT_LED_OFF()
#endif
#define BLINK_R_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
#define BLINK_B_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
#define BLINK_B_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
#define BLINK_R_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
#define BLINK_B_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011)
#define BLINK_R_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000)
#define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111
#define BLINK_R_HEARTBEAT_PATTERN 0
#define BLINK_B_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000100100111111 : \
x == NOTIFY_NOK ? 0b0000000000111111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
#define BLINK_R_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000000000001111 : \
x == NOTIFY_NOK ? 0b0011000011001111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
// led notification handling
static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
static volatile FlightStatusData currentFlightStatus;
static volatile bool started = false;
static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
#ifdef PIOS_LED_ALARM
static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
#endif // PIOS_LED_ALARM
static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
void NotificationUpdateStatus()
{
started = true;
// get values to be used for led handling
FlightStatusGet((FlightStatusData *)&currentFlightStatus);
currentAlarmLevel = AlarmsGetHighestSeverity();
if (nextNotification == NOTIFY_NONE) {
nextNotification = PIOS_NOTIFY_GetActiveNotification(true);
}
}
void NotificationOnboardLedsRun()
{
static portTickType lastRunTimestamp;
static uint16_t r_pattern;
static uint16_t b_pattern;
static uint8_t cycleCount; // count the number of cycles
static uint8_t lastFlightMode = -1;
static bool forceShowFlightMode = false;
static pios_notify_notification runningNotification = NOTIFY_NONE;
static enum {
STATUS_NOTIFY,
STATUS_ALARM,
STATUS_FLIGHTMODE, // flightMode/HeartBeat
STATUS_LENGHT
} status;
if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
return;
}
lastRunTimestamp = xTaskGetTickCount();
// the led will show various status information, subdivided in three phases
// - Notification
// - Alarm
// - Flight status
// they are shown using the above priority
// a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end
cycleCount++;
// Notifications are "modal" to other statuses so they takes precedence
if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) {
// read next notification to show
runningNotification = nextNotification;
nextNotification = NOTIFY_NONE;
// Force a notification status
status = STATUS_NOTIFY;
cycleCount = 0; // instantly start a notify cycle
} else {
if (lastFlightMode != currentFlightStatus.FlightMode) {
status = STATUS_FLIGHTMODE;
lastFlightMode = currentFlightStatus.FlightMode;
cycleCount = 0; // instantly start a flightMode cycle
forceShowFlightMode = true;
}
}
// check if a phase has just finished
if (cycleCount & 0x10) {
HEARTBEAT_LED_OFF();
ALARM_LED_OFF();
cycleCount = 0x0;
forceShowFlightMode = false;
// Notification has been just shown, cleanup
if (status == STATUS_NOTIFY) {
runningNotification = NOTIFY_NONE;
}
status = (status + 1) % STATUS_LENGHT;
}
if (status == STATUS_NOTIFY) {
if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
// no notifications, advance
status++;
}
}
// Handles Alarm display
if (status == STATUS_ALARM) {
#ifdef PIOS_LED_ALARM
if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
// no alarms, advance
status++;
}
#else
// no alarms leds, advance
status++;
#endif // PIOS_LED_ALARM
}
// **** Handles flightmode display
if (status == STATUS_FLIGHTMODE && !cycleCount) {
if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
handleFlightMode(&r_pattern, &b_pattern);
} else {
handleHeartbeat(&r_pattern, &b_pattern);
}
}
// led output
if (b_pattern & 0x1) {
HEARTBEAT_LED_ON();
} else {
HEARTBEAT_LED_OFF();
}
if (r_pattern & 0x1) {
ALARM_LED_ON();
} else {
ALARM_LED_OFF();
}
r_pattern >>= 1;
b_pattern >>= 1;
}
#if defined(PIOS_LED_ALARM)
static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
{
if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
return false;
}
*b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
*r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
return true;
}
#endif /* PIOS_LED_ALARM */
static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
{
if (runningNotification == NOTIFY_NONE) {
return false;
}
*b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
*r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
return true;
}
static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern)
{
// Flash the heartbeat LED
uint8_t flightmode = currentFlightStatus.FlightMode;
if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
*b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
*r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
} else {
*b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
*r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
}
}
static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern)
{
// Flash the heartbeat LED
*b_pattern = BLINK_B_HEARTBEAT_PATTERN;
*r_pattern = BLINK_R_HEARTBEAT_PATTERN;
}

9
flight/modules/Attitude/attitude.c
View File

@ -63,7 +63,7 @@
#include "taskinfo.h"
#include "CoordinateConversions.h"
#include <pios_notify.h>
// Private constants
#define STACK_SIZE_BYTES 540
@ -252,6 +252,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1.0f;
accelKi = 0.0f;
@ -259,6 +260,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
@ -693,8 +695,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
}
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
attitudeSettings.BoardRotation.Yaw == 0) {
if (fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
rotate = 0;
// Shouldn't be used but to be safe

88
flight/modules/Sensors/sensors.c
View File

@ -84,10 +84,13 @@ AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
// temp coefficient to calculate gyro bias
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
static float R[3][3] = {
{ 0 }
};
@ -119,6 +122,7 @@ int32_t SensorsInitialize(void)
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -359,7 +363,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -389,7 +392,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -411,20 +413,16 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
@ -446,12 +444,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
@ -465,18 +460,49 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 &&
attitudeSettings.BoardRotation.Yaw == 0) {
if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f
&& fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
rotate = 0;
} else {
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
R[1][0] * cal.mag_transform.r0c1 +
R[2][0] * cal.mag_transform.r0c2;
mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
R[1][1] * cal.mag_transform.r0c1 +
R[2][1] * cal.mag_transform.r0c2;
mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
R[1][2] * cal.mag_transform.r0c1 +
R[2][2] * cal.mag_transform.r0c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
R[1][0] * cal.mag_transform.r1c1 +
R[2][0] * cal.mag_transform.r1c2;
mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
R[1][1] * cal.mag_transform.r1c1 +
R[2][1] * cal.mag_transform.r1c2;
mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
R[1][2] * cal.mag_transform.r1c1 +
R[2][2] * cal.mag_transform.r1c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
R[1][0] * cal.mag_transform.r2c1 +
R[2][0] * cal.mag_transform.r2c2;
mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
R[1][1] * cal.mag_transform.r2c1 +
R[2][1] * cal.mag_transform.r2c2;
mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
R[1][2] * cal.mag_transform.r2c1 +
R[2][2] * cal.mag_transform.r2c2;
}
/**
* @}

16
flight/modules/Stabilization/outerloop.c
View File

@ -114,9 +114,19 @@ static void stabilizationOuterloopTask()
float q_desired[4];
float q_error[4];
rpy_desired[0] = stabilizationDesiredAxis[0];
rpy_desired[1] = stabilizationDesiredAxis[1];
rpy_desired[2] = stabilizationDesiredAxis[2];
for (t = 0; t < 3; t++) {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
rpy_desired[t] = stabilizationDesiredAxis[t];
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
break;
}
}
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);

4
flight/modules/StateEstimation/filtercf.c
View File

@ -42,7 +42,7 @@
#include <revocalibration.h>
#include <CoordinateConversions.h>
#include <pios_notify.h>
// Private constants
#define STACK_REQUIRED 512
@ -295,6 +295,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
@ -303,6 +304,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
this->init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (this->init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&this->attitudeSettings);

7
flight/modules/StateEstimation/filterekf.c
View File

@ -36,6 +36,7 @@
#include <ekfconfiguration.h>
#include <ekfstatevariance.h>
#include <attitudestate.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <insgps.h>
@ -348,7 +349,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
sensors |= MAG_SENSORS;
}
}
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {

71
flight/modules/StateEstimation/filtermag.c
View File

@ -33,6 +33,8 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -43,9 +45,13 @@
// Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
RevoSettingsData revoSettings;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
float magBe2;
float magBias[3];
};
// Private variables
@ -54,6 +60,7 @@ struct data {
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -70,9 +77,13 @@ static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->warningcount = this->errorcount = 0;
HomeLocationBeGet(this->homeLocationBe);
// magBe2 holds the squared magnetic vector length (extpected)
this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
return 0;
}
@ -81,6 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
checkMagValidity(this, state->mag);
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
@ -89,6 +101,53 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
/**
* check validity of magnetometers
*/
static void checkMagValidity(struct data *this, float mag[3])
{
#define ALARM_THRESHOLD 5
// mag2 holds the actual magnetic vector length (squared)
float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
// warning and error thresholds
// avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
//
// actual = |mag|
// minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
// maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
// minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
// maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
//
float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
// set errors
if (minWarning2 < mag2 && mag2 < maxWarning2) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (minError2 < mag2 && mag2 < maxError2) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
this->warningcount++;
}
} else {
if (this->errorcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
} else {
this->errorcount++;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
@ -137,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3])
}
#else // if 0
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = this->homeLocation.Be[2];
const float Rxy = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
const float Rz = this->homeLocationBe[2];
const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3];

13
flight/modules/StateEstimation/stateestimation.c
View File

@ -161,17 +161,14 @@ static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter,
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
.filter = &cfFilter,
.next = NULL,
}
}
}

62
flight/modules/System/systemmod.c
View File

@ -42,7 +42,7 @@
#include <pios_struct_helper.h>
// private includes
#include "inc/systemmod.h"
#include "notification.h"
// UAVOs
#include <objectpersistence.h>
@ -72,22 +72,15 @@
#endif
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif
#define SYSTEM_UPDATE_PERIOD_MS 250
#if defined(PIOS_SYSTEM_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1024
#define STACK_SIZE_BYTES 1024
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Private types
@ -98,6 +91,7 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
static bool mallocFailed;
static HwSettingsData bootHwSettings;
static struct PIOS_FLASHFS_Stats fsStats;
// Private functions
static void objectUpdatedCb(UAVObjEvent *ev);
static void hwSettingsUpdatedCb(UAVObjEvent *ev);
@ -170,8 +164,6 @@ MODULE_INITCALL(SystemModInitialize, 0);
*/
static void systemTask(__attribute__((unused)) void *parameters)
{
uint8_t cycleCount = 0;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
@ -189,9 +181,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
@ -204,13 +193,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
TaskInfoData taskInfoData;
CallbackInfoData callbackInfoData;
#endif
// Main system loop
while (1) {
NotificationUpdateStatus();
// Update the system statistics
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
// if(cycleCount == 1){
updateStats();
// Update the system alarms
updateSystemAlarms();
@ -230,35 +216,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
// }
#endif
// }
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
uint8_t armingStatus;
FlightStatusArmedGet(&armingStatus);
if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) ||
(armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
#if defined(PIOS_LED_ALARM)
if (AlarmsHasCritical()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else if ((AlarmsHasErrors() && (cycleCount & 0x1)) ||
(!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
#endif /* PIOS_LED_ALARM */
UAVObjEvent ev;
int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
int delayTime = SYSTEM_UPDATE_PERIOD_MS;
#if defined(PIOS_INCLUDE_RFM22B)
@ -649,11 +610,12 @@ static void updateSystemAlarms()
}
/**
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
* Called by the RTOS when the CPU is idle,
*/
void vApplicationIdleHook(void)
{}
{
NotificationOnboardLedsRun();
}
/**
* Called by the RTOS when a stack overflow is detected.
*/

5
flight/modules/Telemetry/telemetry.c
View File

@ -661,11 +661,10 @@ static void updateTelemetryStats()
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
// Update the telemetry alarm
// TODO: check whether is there any error condition worth raising an alarm
// Disconnection is actually a normal (non)working status so it is not raising alarms anymore.
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
// Update object

49
flight/pios/common/pios_notify.c Normal file
View File

@ -0,0 +1,49 @@
/**
******************************************************************************
*
* @file pios_notify.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Handles user notifications.
* --
* @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_notify.h"
static volatile pios_notify_notification currentNotification = NOTIFY_NONE;
static volatile pios_notify_priority currentPriority;
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority)
{
if (currentNotification == NOTIFY_NONE || currentPriority < priority) {
currentPriority = priority;
currentNotification = notification;
}
}
pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear)
{
pios_notify_notification ret = currentNotification;
if (clear && ret != NOTIFY_NONE) {
currentNotification = NOTIFY_NONE;
}
return ret;
}

58
flight/pios/inc/pios_notify.h Normal file
View File

@ -0,0 +1,58 @@
/**
******************************************************************************
*
* @file pios_notify.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Handles user notifications.
* --
* @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_NOTIFY_H_
#define PIOS_NOTIFY_H_
#include <stdbool.h>
typedef enum {
NOTIFY_NONE = 0,
NOTIFY_OK,
NOTIFY_NOK,
NOTIFY_DRAW_ATTENTION
} pios_notify_notification;
typedef enum {
NOTIFY_PRIORITY_CRITICAL = 2,
NOTIFY_PRIORITY_REGULAR = 1,
NOTIFY_PRIORITY_LOW = 0,
} pios_notify_priority;
/**
* start a new notification. If a notification is active it will be overwritten if its priority is lower than the new one.
* The new will be discarded otherwise
* @param notification kind of notification
* @param priority priority of the new notification
*/
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority);
/**
* retrieve any active notification
* @param clear
* @return
*/
pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear);
#endif /* PIOS_NOTIFY_H_ */

560
flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c Normal file
View File

@ -0,0 +1,560 @@
/**
******************************************************************************
* @file system_stm32f4xx.c
* @author MCD Application Team
* @version V1.1.0
* @date 11-January-2013
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F4xx devices,
* and is generated by the clock configuration tool
* stm32f4xx_Clock_Configuration_V1.1.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define
* in "stm32f4xx.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
*=============================================================================
* Supported STM32F40xx/41xx/427x/437x devices
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 4
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*-----------------------------------------------------------------------------
* PLL_M | 10
*-----------------------------------------------------------------------------
* PLL_N | 420
*-----------------------------------------------------------------------------
* PLL_P | 2
*-----------------------------------------------------------------------------
* PLL_Q | 7
*-----------------------------------------------------------------------------
* PLLI2S_N | NA
*-----------------------------------------------------------------------------
* PLLI2S_R | NA
*-----------------------------------------------------------------------------
* I2S input clock | NA
*-----------------------------------------------------------------------------
* VDD(V) | 3.3
*-----------------------------------------------------------------------------
* Main regulator output voltage | Scale1 mode
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 5
*-----------------------------------------------------------------------------
* Prefetch Buffer | ON
*-----------------------------------------------------------------------------
* Instruction cache | ON
*-----------------------------------------------------------------------------
* Data cache | ON
*-----------------------------------------------------------------------------
* Require 48MHz for USB OTG FS, | Enabled
* SDIO and RNG clock |
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f4xx_system
* @{
*/
/** @addtogroup STM32F4xx_System_Private_Includes
* @{
*/
#include "stm32f4xx.h"
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Defines
* @{
*/
/************************* Miscellaneous Configuration ************************/
/*!< Uncomment the following line if you need to use external SRAM mounted
on STM324xG_EVAL/STM324x7I_EVAL boards as data memory */
/* #define DATA_IN_ExtSRAM */
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
/******************************************************************************/
/************************* PLL Parameters *************************************/
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
#define PLL_M 10
#define PLL_N 420
/* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 2
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
/******************************************************************************/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Variables
* @{
*/
uint32_t SystemCoreClock = 168000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
* @{
*/
static void SetSysClock(void);
#ifdef DATA_IN_ExtSRAM
static void SystemInit_ExtMemCtl(void);
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR = 0x00000000;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset PLLCFGR register */
RCC->PLLCFGR = 0x24003010;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
#ifdef DATA_IN_ExtSRAM
SystemInit_ExtMemCtl();
#endif /* DATA_IN_ExtSRAM */
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
SetSysClock();
/* Configure the Vector Table location add offset address ------------------*/
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value
* 25 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
SYSCLK = PLL_VCO / PLL_P
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
else
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp;
break;
default:
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK frequency --------------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @Note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
static void SetSysClock(void)
{
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR |= PWR_CR_VOS;
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @brief Setup the external memory controller. Called in startup_stm32f4xx.s
* before jump to __main
* @param None
* @retval None
*/
#ifdef DATA_IN_ExtSRAM
/**
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external SRAM mounted on STM324xG_EVAL/STM324x7I boards
* This SRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
void SystemInit_ExtMemCtl(void)
{
/*-- GPIOs Configuration -----------------------------------------------------*/
/*
+-------------------+--------------------+------------------+------------------+
+ SRAM pins assignment +
+-------------------+--------------------+------------------+------------------+
| PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 |
| PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 |
| PD4 <-> FSMC_NOE | PE2 <-> FSMC_A23 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 |
| PD5 <-> FSMC_NWE | PE3 <-> FSMC_A19 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 |
| PD8 <-> FSMC_D13 | PE4 <-> FSMC_A20 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 |
| PD9 <-> FSMC_D14 | PE5 <-> FSMC_A21 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 |
| PD10 <-> FSMC_D15 | PE6 <-> FSMC_A22 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 |
| PD11 <-> FSMC_A16 | PE7 <-> FSMC_D4 | PF13 <-> FSMC_A7 |------------------+
| PD12 <-> FSMC_A17 | PE8 <-> FSMC_D5 | PF14 <-> FSMC_A8 |
| PD13 <-> FSMC_A18 | PE9 <-> FSMC_D6 | PF15 <-> FSMC_A9 |
| PD14 <-> FSMC_D0 | PE10 <-> FSMC_D7 |------------------+
| PD15 <-> FSMC_D1 | PE11 <-> FSMC_D8 |
+-------------------| PE12 <-> FSMC_D9 |
| PE13 <-> FSMC_D10 |
| PE14 <-> FSMC_D11 |
| PE15 <-> FSMC_D12 |
+--------------------+
*/
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
RCC->AHB1ENR |= 0x00000078;
/* Connect PDx pins to FSMC Alternate function */
GPIOD->AFR[0] = 0x00cc00cc;
GPIOD->AFR[1] = 0xcccccccc;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xaaaa0a0a;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xffff0f0f;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FSMC Alternate function */
GPIOE->AFR[0] = 0xcccccccc;
GPIOE->AFR[1] = 0xcccccccc;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xaaaaaaaa;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xffffffff;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FSMC Alternate function */
GPIOF->AFR[0] = 0x00cccccc;
GPIOF->AFR[1] = 0xcccc0000;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xaa000aaa;
/* Configure PFx pins speed to 100 MHz */
GPIOF->OSPEEDR = 0xff000fff;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FSMC Alternate function */
GPIOG->AFR[0] = 0x00cccccc;
GPIOG->AFR[1] = 0x000000c0;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0x00080aaa;
/* Configure PGx pins speed to 100 MHz */
GPIOG->OSPEEDR = 0x000c0fff;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/*-- FSMC Configuration ------------------------------------------------------*/
/* Enable the FSMC interface clock */
RCC->AHB3ENR |= 0x00000001;
/* Configure and enable Bank1_SRAM2 */
FSMC_Bank1->BTCR[2] = 0x00001011;
FSMC_Bank1->BTCR[3] = 0x00000201;
FSMC_Bank1E->BWTR[2] = 0x0fffffff;
/*
Bank1_SRAM2 is configured as follow:
p.FSMC_AddressSetupTime = 1;
p.FSMC_AddressHoldTime = 0;
p.FSMC_DataSetupTime = 2;
p.FSMC_BusTurnAroundDuration = 0;
p.FSMC_CLKDivision = 0;
p.FSMC_DataLatency = 0;
p.FSMC_AccessMode = FSMC_AccessMode_A;
FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM;
FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
*/
}
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

4
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -48,9 +48,13 @@ OPTMODULES += Osd/osdoutput
#OPTMODULES += Altitude
#OPTMODULES += Fault
SRC += $(FLIGHTLIB)/notification.c
# Include all camera options
CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
# Erase flash firmware should be buildable from command line
ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH

50
flight/targets/boards/discoveryf4bare/board-info.mk Normal file
View File

@ -0,0 +1,50 @@
BOARD_TYPE := 0x09
BOARD_REVISION := 0x03
BOOTLOADER_VERSION := 0x06
HW_TYPE := 0x00
MCU := cortex-m4
CHIP := STM32F407VGT
BOARD := STM32F4xx_DI
MODEL := HD
MODEL_SUFFIX :=
OPENOCD_JTAG_CONFIG := stlink-v2.cfg
OPENOCD_CONFIG := stm32f4xx.stlink.cfg
# Flash memory map for Revolution:
# Sector start size use
# 0 0x0800 0000 16k BL
# 1 0x0800 4000 16k BL
# 2 0x0800 8000 16k EE
# 3 0x0800 C000 16k EE
# 4 0x0801 0000 64k Unused
# 5 0x0802 0000 128k FW
# 6 0x0804 0000 128k FW
# 7 0x0806 0000 128k FW
# 8 0x0808 0000 128k Unused
# 9 0x080A 0000 128k Unused
# 10 0x080C 0000 128k Unused ..
# 11 0x080E 0000 128k Unused
# Note: These must match the values in link_$(BOARD)_memory.ld
BL_BANK_BASE := 0x08000000 # Start of bootloader flash
BL_BANK_SIZE := 0x00008000 # Should include BD_INFO region
# 16KB for settings storage
EE_BANK_BASE := 0x08008000 # EEPROM storage area
EE_BANK_SIZE := 0x00008000 # Size of EEPROM storage area
USER_EE_BANK_BASE := 0x080C0000 # EEPROM storage area
USER_EE_BANK_SIZE := 0x00040000 # Size of EEPROM storage area
# Leave the remaining 64KB sectors for other uses
FW_BANK_BASE := 0x08020000 # Start of firmware flash
FW_BANK_SIZE := 0x000A0000 # Should include FW_DESC_SIZE
FW_DESC_SIZE := 0x00000064
OSCILLATOR_FREQ := 8000000
SYSCLK_FREQ := 168000000

1812
flight/targets/boards/discoveryf4bare/board_hw_defs.c Normal file
View File

@ -0,0 +1,1812 @@
/**
******************************************************************************
* @file board_hw_defs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @brief Defines board specific static initializers for hardware for the Revolution board.
*****************************************************************************/
/*
* 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
*/
#if defined(PIOS_INCLUDE_LED)
#include <pios_led_priv.h>
static const struct pios_gpio pios_leds[] = {
[PIOS_LED_HEARTBEAT] = {
.pin = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.active_low = false
},
[PIOS_LED_ALARM] = {
.pin = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.active_low = false
},
[PIOS_LED_D1] = {
.pin = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.active_low = false
},
[PIOS_LED_D2] = {
.pin = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.active_low = false
},
};
static const struct pios_gpio_cfg pios_led_cfg = {
.gpios = pios_leds,
.num_gpios = NELEMENTS(pios_leds),
};
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision)
{
return &pios_led_cfg;
}
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
#if defined(PIOS_OVERO_SPI)
/* SPI2 Interface
* - Used for Flexi/IO/Overo communications
3: PB12 = SPI2 NSS, CAN2 RX
4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS
5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS
6: PB15 = SPI2 MOSI, TIM12 CH2
*/
#include <pios_overo_priv.h>
void PIOS_OVERO_irq_handler(void);
void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler")));
static const struct pios_overo_cfg pios_overo_cfg = {
.regs = SPI2,
.remap = GPIO_AF_SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
},
.use_crc = false,
.dma = {
.irq = {
// Note this is the stream ID that triggers interrupts (in this case TX)
.flags = (DMA_IT_TCIF7),
.init = {
.NVIC_IRQChannel = DMA1_Stream7_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Stream0,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralToMemory,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_Medium,
// TODO: Enable FIFO
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
.tx = {
.channel = DMA1_Stream7,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_Medium,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.slave_count = 1,
.ssel = {
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
}
},
};
uint32_t pios_overo_id = 0;
void PIOS_OVERO_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_OVERO_DMA_irq_handler(pios_overo_id);
}
#endif /* PIOS_OVERO_SPI */
/*
* SPI1 Interface
* Used for MPU6000 gyro and accelerometer
*/
void PIOS_SPI_gyro_irq_handler(void);
void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
static const struct pios_spi_cfg pios_spi_gyro_cfg = {
.regs = SPI1,
.remap = GPIO_AF_SPI1,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16,
},
.use_crc = false,
.dma = {
.irq = {
.flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0),
.init = {
.NVIC_IRQChannel = DMA2_Stream0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA2_Stream0,
.init = {
.DMA_Channel = DMA_Channel_3,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralToMemory,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
/* .DMA_FIFOThreshold */
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
.tx = {
.channel = DMA2_Stream3,
.init = {
.DMA_Channel = DMA_Channel_3,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
/* .DMA_FIFOThreshold */
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
},
.sclk = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.miso = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.mosi = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.slave_count = 1,
.ssel = {
{
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
}
}
}
};
static uint32_t pios_spi_gyro_id;
void PIOS_SPI_gyro_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_gyro_id);
}
#if false
/*
* SPI3 Interface
* Used for Flash and the RFM22B
*/
void PIOS_SPI_telem_flash_irq_handler(void);
void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler")));
void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler")));
static const struct pios_spi_cfg pios_spi_telem_flash_cfg = {
.regs = SPI3,
.remap = GPIO_AF_SPI3,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_Low,
.SPI_CPHA = SPI_CPHA_1Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
},
.use_crc = false,
.dma = {
.irq = {
// Note this is the stream ID that triggers interrupts (in this case RX)
.flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0),
.init = {
.NVIC_IRQChannel = DMA1_Stream0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Stream0,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR),
.DMA_DIR = DMA_DIR_PeripheralToMemory,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
// TODO: Enable FIFO
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
.tx = {
.channel = DMA1_Stream5,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR),
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
},
.sclk = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.miso = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.mosi = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.slave_count = 2,
.ssel = {
{ // RFM22b
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
}
},
{ // Flash
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
}
},
},
};
uint32_t pios_spi_telem_flash_id;
void PIOS_SPI_telem_flash_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id);
}
#if defined(PIOS_INCLUDE_RFM22B)
#include <pios_rfm22b_priv.h>
static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = {
.vector = PIOS_RFM22_EXT_Int,
.line = EXTI_Line2,
.pin = {
.gpio = GPIOD,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.irq = {
.init = {
.NVIC_IRQChannel = EXTI2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = EXTI_Line2, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Falling,
.EXTI_LineCmd = ENABLE,
},
},
};
const struct pios_rfm22b_cfg pios_rfm22b_rm1_cfg = {
.spi_cfg = &pios_spi_telem_flash_cfg,
.exti_cfg = &pios_exti_rfm22b_cfg,
.RFXtalCap = 0x7f,
.slave_num = 0,
.gpio_direction = GPIO0_RX_GPIO1_TX,
};
const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = {
.spi_cfg = &pios_spi_telem_flash_cfg,
.exti_cfg = &pios_exti_rfm22b_cfg,
.RFXtalCap = 0x7f,
.slave_num = 0,
.gpio_direction = GPIO0_TX_GPIO1_RX,
};
const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision)
{
switch (board_revision) {
case 2:
return &pios_rfm22b_rm1_cfg;
break;
case 3:
return &pios_rfm22b_rm2_cfg;
break;
default:
PIOS_DEBUG_Assert(0);
}
return NULL;
}
#endif /* PIOS_INCLUDE_RFM22B */
#endif /* false */
#endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_FLASH)
#include "pios_flashfs_logfs_priv.h"
#include "pios_flash_jedec_priv.h"
#include "pios_flash_internal_priv.h"
static const struct flashfs_logfs_cfg flashfs_external_cfg = {
.fs_magic = 0x99abceef,
.total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */
.arena_size = 0x00010000, /* 256 * slot size */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = 0, /* start at the beginning of the chip */
.sector_size = 0x00010000, /* 64K bytes */
.page_size = 0x00000100, /* 256 bytes */
};
static const struct pios_flash_internal_cfg flash_internal_system_cfg = {};
static const struct flashfs_logfs_cfg flashfs_internal_cfg = {
.fs_magic = 0x99abcfef,
.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */
.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = EE_BANK_BASE, /* start after the bootloader */
.sector_size = 0x00004000, /* 16K bytes */
.page_size = 0x00004000, /* 16K bytes */
};
static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = {
.fs_magic = 0x99abcfef,
.total_fs_size = USER_EE_BANK_SIZE, /* 128K bytes (2x16KB sectors) */
.arena_size = 0x00020000, /* 64 * slot size = 16K bytes = 1 sector */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = USER_EE_BANK_BASE, /* start after the bootloader */
.sector_size = 0x00020000, /* 128K bytes */
.page_size = 0x00020000, /* 128K bytes */
};
#endif /* PIOS_INCLUDE_FLASH */
#include <pios_usart_priv.h>
#ifdef PIOS_INCLUDE_COM_TELEM
/*
* MAIN USART
*/
static const struct pios_usart_cfg pios_usart_main_cfg = {
.regs = USART1,
.remap = GPIO_AF_USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#endif /* PIOS_INCLUDE_COM_TELEM */
#ifdef PIOS_INCLUDE_DSM
#include "pios_dsm_priv.h"
static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
.regs = USART1,
.remap = GPIO_AF_USART1,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
// Because of the inverter on the main port this will not
// work. Notice the mode is set to IN to maintain API
// compatibility but protect the pins
static const struct pios_dsm_cfg pios_dsm_main_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
};
#endif /* PIOS_INCLUDE_DSM */
#include <pios_sbus_priv.h>
#if defined(PIOS_INCLUDE_SBUS)
/*
* S.Bus USART
*/
#include <pios_sbus_priv.h>
static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
.regs = USART1,
.remap = GPIO_AF_USART1,
.init = {
.USART_BaudRate = 100000,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_Even,
.USART_StopBits = USART_StopBits_2,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
};
#endif /* PIOS_INCLUDE_SBUS */
// Need this defined regardless to be able to turn it off
static const struct pios_sbus_cfg pios_sbus_cfg = {
/* Inverter configuration */
.inv = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.gpio_inv_enable = Bit_SET,
.gpio_inv_disable = Bit_RESET,
.gpio_clk_func = RCC_AHB1PeriphClockCmd,
.gpio_clk_periph = RCC_AHB1Periph_GPIOC,
};
#ifdef PIOS_INCLUDE_COM_FLEXI
/*
* FLEXI PORT
*/
static const struct pios_usart_cfg pios_usart_flexi_cfg = {
.regs = USART3,
.remap = GPIO_AF_USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#endif /* PIOS_INCLUDE_COM_FLEXI */
#ifdef PIOS_INCLUDE_DSM
#include "pios_dsm_priv.h"
static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
.regs = USART3,
.remap = GPIO_AF_USART3,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
.bind = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
};
#endif /* PIOS_INCLUDE_DSM */
/*
* HK OSD
*/
static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
.regs = USART1,
.remap = GPIO_AF_USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
.regs = USART3,
.remap = GPIO_AF_USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void);
void PIOS_I2C_mag_pressureadapter_er_irq_handler(void);
void I2C1_EV_IRQHandler()
__attribute__((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler()
__attribute__((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = {
.regs = I2C1,
.remap = GPIO_AF_I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 400000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_mag_pressure_adapter_id;
void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id);
}
void PIOS_I2C_mag_pressure_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id);
}
void PIOS_I2C_flexiport_adapter_ev_irq_handler(void);
void PIOS_I2C_flexiport_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = {
.regs = I2C2,
.remap = GPIO_AF_I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 400000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_flexiport_adapter_id;
void PIOS_I2C_flexiport_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id);
}
void PIOS_I2C_flexiport_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id);
}
void PIOS_I2C_pressure_adapter_ev_irq_handler(void);
void PIOS_I2C_pressure_adapter_er_irq_handler(void);
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler(void);
void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1
// For some reason it's acting like crystal is 16 Mhz. This clock is then divided
// by another 16 to give a nominal 62.5 khz clock
.prescaler = 100, // Every 100 cycles gives 625 Hz
.irq = {
.init = {
.NVIC_IRQChannel = RTC_WKUP_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler(void)
{
PIOS_RTC_irq_handler();
}
#endif /* if defined(PIOS_INCLUDE_RTC) */
#include "pios_tim_priv.h"
static const TIM_TimeBaseInitTypeDef tim_3_5_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
.TIM_RepetitionCounter = 0x0000,
};
static const TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_3_cfg = {
.timer = TIM3,
.time_base_init = &tim_3_5_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_5_cfg = {
.timer = TIM5,
.time_base_init = &tim_3_5_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_9_cfg = {
.timer = TIM9,
.time_base_init = &tim_9_10_11_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_10_cfg = {
.timer = TIM10,
.time_base_init = &tim_9_10_11_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_UP_TIM10_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_11_cfg = {
.timer = TIM11,
.time_base_init = &tim_9_10_11_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
// Set up timers that only have inputs on APB1
// TIM2,3,4,5,6,7,12,13,14
static const TIM_TimeBaseInitTypeDef tim_apb1_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF,
.TIM_RepetitionCounter = 0x0000,
};
// Set up timers that only have inputs on APB2
// TIM1,8,9,10,11
static const TIM_TimeBaseInitTypeDef tim_apb2_time_base = {
.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF,
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_1_cfg = {
.timer = TIM1,
.time_base_init = &tim_apb2_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_4_cfg = {
.timer = TIM4,
.time_base_init = &tim_apb1_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_8_cfg = {
.timer = TIM8,
.time_base_init = &tim_apb2_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM8_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_12_cfg = {
.timer = TIM12,
.time_base_init = &tim_apb1_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
/**
* Pios servo configuration structures
* Using TIM3, TIM9, TIM2, TIM5
*/
#include <pios_servo_priv.h>
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
{
.timer = TIM3,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource0,
},
.remap = GPIO_AF_TIM3,
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource1,
},
.remap = GPIO_AF_TIM3,
},
{
.timer = TIM9,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource3,
},
.remap = GPIO_AF_TIM9,
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource2,
},
.remap = GPIO_AF_TIM2,
},
{
.timer = TIM5,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource1,
},
.remap = GPIO_AF_TIM5,
},
{
.timer = TIM5,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource0,
},
.remap = GPIO_AF_TIM5,
},
// PWM pins on FlexiIO(receiver) port
{
// * 6: PB15 = SPI2 MOSI, TIM12 CH2
.timer = TIM12,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource15,
},
.remap = GPIO_AF_TIM12,
},
{
// * 7: PC6 = TIM8 CH1, USART6 TX
.timer = TIM8,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource6,
},
.remap = GPIO_AF_TIM8,
},
{
// * 8: PC7 = TIM8 CH2, USART6 RX
.timer = TIM8,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource7,
},
.remap = GPIO_AF_TIM8,
},
{
// * 9: PC8 = TIM8 CH3
.timer = TIM8,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource8,
},
.remap = GPIO_AF_TIM8,
},
{
// * 10: PC9 = TIM8 CH4
.timer = TIM8,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource9,
},
.remap = GPIO_AF_TIM8,
},
{
// * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS
.timer = TIM12,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource14,
},
.remap = GPIO_AF_TIM12,
},
};
#define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6
#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM 11
#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12
const struct pios_servo_cfg pios_servo_cfg_out = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT,
};
// All servo outputs, servo input ch1 ppm, ch2-6 outputs
const struct pios_servo_cfg pios_servo_cfg_out_in_ppm = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM,
};
// All servo outputs, servo inputs ch1-6 Outputs
const struct pios_servo_cfg pios_servo_cfg_out_in = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN,
};
/*
* PWM Inputs
* TIM1, TIM8, TIM12
*/
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
#include <pios_pwm_priv.h>
static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
{
.timer = TIM12,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource14,
},
.remap = GPIO_AF_TIM12,
},
{
.timer = TIM12,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource15,
},
.remap = GPIO_AF_TIM12,
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource6,
},
.remap = GPIO_AF_TIM8,
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource7,
},
.remap = GPIO_AF_TIM8,
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource8,
},
.remap = GPIO_AF_TIM8,
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource9,
},
.remap = GPIO_AF_TIM8,
},
};
const struct pios_pwm_cfg pios_pwm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_rcvrport_all_channels,
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
};
// this configures outputs 2-6 as pwm inputs
const struct pios_pwm_cfg pios_pwm_ppm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = &pios_tim_rcvrport_all_channels[1],
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1,
};
#endif /* if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) */
/*
* PPM Input
*/
#if defined(PIOS_INCLUDE_PPM)
#include <pios_ppm_priv.h>
static const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
.TIM_Channel = TIM_Channel_1,
},
/* Use only the first channel for ppm */
.channels = &pios_tim_rcvrport_all_channels[0],
.num_channels = 1,
};
#endif // PPM
#if defined(PIOS_INCLUDE_GCSRCVR)
#include "pios_gcsrcvr_priv.h"
#endif /* PIOS_INCLUDE_GCSRCVR */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
#endif /* PIOS_INCLUDE_RCVR */
#if defined(PIOS_INCLUDE_USB)
#include "pios_usb_priv.h"
static const struct pios_usb_cfg pios_usb_discovery_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = OTG_FS_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0, // PriorityGroup=4
.NVIC_IRQChannelCmd = ENABLE,
},
},
.vsense = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_25MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
},
},
.vsense_active_low = false
};
const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision)
{
return &pios_usb_discovery_cfg;
}
#include "pios_usb_board_data_priv.h"
#include "pios_usb_desc_hid_cdc_priv.h"
#include "pios_usb_desc_hid_only_priv.h"
#include "pios_usbhook.h"
#endif /* PIOS_INCLUDE_USB */
#if defined(PIOS_INCLUDE_COM_MSG)
#include <pios_com_msg_priv.h>
#endif /* PIOS_INCLUDE_COM_MSG */
#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC)
#include <pios_usb_hid_priv.h>
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
.data_if = 0,
.data_rx_ep = 1,
.data_tx_ep = 1,
};
#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC)
#include <pios_usb_cdc_priv.h>
const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
.ctrl_if = 0,
.ctrl_tx_ep = 2,
.data_if = 1,
.data_rx_ep = 3,
.data_tx_ep = 3,
};
#include <pios_usb_hid_priv.h>
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
.data_if = 2,
.data_rx_ep = 1,
.data_tx_ep = 1,
};
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */

26
flight/targets/boards/discoveryf4bare/bootloader/Makefile Normal file
View File

@ -0,0 +1,26 @@
#
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
#
# 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 OPENPILOT_IS_COOL
$(error Top level Makefile must be used to build this target)
endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
include $(ROOT_DIR)/make/boot-defs.mk
include $(ROOT_DIR)/make/common-defs.mk

115
flight/targets/boards/discoveryf4bare/bootloader/inc/common.h Normal file
View File

@ -0,0 +1,115 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
*
* @{
* @file common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains various common defines for the BootLoader
* @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 COMMON_H_
#define COMMON_H_
// #include "board.h"
typedef enum {
start, keepgoing,
} DownloadAction;
/**************************************************/
/* OP_DFU states */
/**************************************************/
typedef enum {
DFUidle, // 0
uploading, // 1
wrong_packet_received, // 2
too_many_packets, // 3
too_few_packets, // 4
Last_operation_Success, // 5
downloading, // 6
BLidle, // 7
Last_operation_failed, // 8
uploadingStarting, // 9
outsideDevCapabilities, // 10
CRC_Fail, // 11
failed_jump,
// 12
} DFUStates;
/**************************************************/
/* OP_DFU commands */
/**************************************************/
typedef enum {
Reserved, // 0
Req_Capabilities, // 1
Rep_Capabilities, // 2
EnterDFU, // 3
JumpFW, // 4
Reset, // 5
Abort_Operation, // 6
Upload, // 7
Op_END, // 8
Download_Req, // 9
Download, // 10
Status_Request, // 11
Status_Rep
// 12
} DFUCommands;
typedef enum {
High_Density, Medium_Density
} DeviceType;
/**************************************************/
/* OP_DFU transfer types */
/**************************************************/
typedef enum {
FW, // 0
Descript
// 2
} DFUTransfer;
/**************************************************/
/* OP_DFU transfer port */
/**************************************************/
typedef enum {
Usb, // 0
Serial
// 2
} DFUPort;
/**************************************************/
/* OP_DFU programable programable HW types */
/**************************************************/
typedef enum {
Self_flash, // 0
Remote_flash_via_spi
// 1
} DFUProgType;
/**************************************************/
/* OP_DFU programable sources */
/**************************************************/
#define USB 0
#define SPI 1
#define DownloadDelay 100000
#define MAX_DEL_RETRYS 3
#define MAX_WRI_RETRYS 3
#endif /* COMMON_H_ */

44
flight/targets/boards/discoveryf4bare/bootloader/inc/pios_config.h Normal file
View File

@ -0,0 +1,44 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @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_CONFIG_H
#define PIOS_CONFIG_H
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_USB
#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
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
#endif /* PIOS_CONFIG_H */

52
flight/targets/boards/discoveryf4bare/bootloader/inc/pios_usb_board_data.h Normal file
View File

@ -0,0 +1,52 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB 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_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2
#include <pios_usb_defs.h> /* struct usb_* */
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL)
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
/*
* The bootloader uses a simplified report structure
* BL: <REPORT_ID><DATA>...<DATA>
* FW: <REPORT_ID><LENGTH><DATA>...<DATA>
* This define changes the behaviour in pios_usb_hid.c
*/
#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
#endif /* PIOS_USB_BOARD_DATA_H */

252
flight/targets/boards/discoveryf4bare/bootloader/main.c Normal file
View File

@ -0,0 +1,252 @@
/**
******************************************************************************
* @addtogroup RevolutionBL Revolution BootLoader
* @brief These files contain the code to the Revolution Bootloader.
*
* @{
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This is the file with the main function of the Revolution BootLoader
* @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>
#include <pios_board_info.h>
#include <op_dfu.h>
#include <pios_iap.h>
#include <fifo_buffer.h>
#include <pios_com_msg.h>
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
#include <stdbool.h>
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
void check_bor();
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
/* Private typedef -----------------------------------------------------------*/
typedef void (*pFunction)(void);
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
////////////////////////////////////////
uint8_t tempcount = 0;
/* Extern variables ----------------------------------------------------------*/
DFUStates DeviceState;
int16_t status = 0;
bool JumpToApp = false;
bool GO_dfu = false;
bool USB_connected = false;
bool User_DFU_request = false;
static uint8_t mReceive_Buffer[63];
/* Private function prototypes -----------------------------------------------*/
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
uint8_t processRX();
void jump_to_app();
int main()
{
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
// Make sure the brown out reset value for this chip
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = true;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == true) || (User_DFU_request == true);
if (GO_dfu == true) {
if (User_DFU_request == true) {
DeviceState = DFUidle;
} else {
DeviceState = BLidle;
}
} else {
JumpToApp = true;
}
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (true) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == true) {
jump_to_app();
}
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default: // error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
}
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
if (stopwatch > 50 * 1000 * 1000) {
stopwatch = 0;
}
if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) {
JumpToApp = true;
}
processRX();
DataDownload(start);
}
}
void jump_to_app()
{
const struct pios_board_info *bdinfo = &pios_board_info_blob;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
// Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack
uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000;
// Check for the two possible irqstack locations (sram or core coupled sram)
if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) {
/* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
PIOS_USBHOOK_Deactivate();
JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4);
Jump_To_Application = (pFunction)JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t *)bdinfo->fw_base);
Jump_To_Application();
} else {
DeviceState = failed_jump;
return;
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count)
{
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX()
{
if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
processComand(mReceive_Buffer);
}
return true;
}
/**
* Check the brown out reset threshold is 2.7 volts and if not
* resets it. This solves an issue that can prevent boards
* powering up with some BEC
*/
void check_bor()
{
uint8_t bor = FLASH_OB_GetBOR();
if (bor != OB_BOR_LEVEL3) {
FLASH_OB_Unlock();
FLASH_OB_BORConfig(OB_BOR_LEVEL3);
FLASH_OB_Launch();
while (FLASH_WaitForLastOperation() == FLASH_BUSY) {
;
}
FLASH_OB_Lock();
while (FLASH_WaitForLastOperation() == FLASH_BUSY) {
;
}
}
}

84
flight/targets/boards/discoveryf4bare/bootloader/pios_board.c Normal file
View File

@ -0,0 +1,84 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @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>
#include <pios_board_info.h>
/*
* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
* the HW definitions to be const and static to limit their
* scope.
*
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/
#include "../board_hw_defs.c"
uint32_t pios_com_telem_usb_id;
static bool board_init_complete = false;
void PIOS_Board_Init()
{
if (board_init_complete) {
return;
}
/* Delay system */
PIOS_DELAY_Init();
const struct pios_board_info *bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED)
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 */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Activate the HID-only USB configuration */
PIOS_USB_DESC_HID_ONLY_Init();
uint32_t pios_usb_id;
PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
PIOS_USBHOOK_Activate();
#endif /* PIOS_INCLUDE_USB */
board_init_complete = true;
}

102
flight/targets/boards/discoveryf4bare/firmware/Makefile Normal file
View File

@ -0,0 +1,102 @@
#
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
#
# 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 OPENPILOT_IS_COOL
$(error Top level Makefile must be used to build this target)
endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# ARM DSP library
USE_DSP_LIB ?= NO
# List of mandatory modules to include
#MODULES += Sensors
#MODULES += Attitude/revolution
#MODULES += StateEstimation # use instead of Attitude
#MODULES += Altitude/revolution
#MODULES += Airspeed
#MODULES += AltitudeHold
#MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl
MODULES += Actuator
MODULES += GPS
MODULES += TxPID
MODULES += CameraStab
MODULES += Battery
MODULES += FirmwareIAP
#MODULES += Radio
MODULES += PathPlanner
MODULES += FixedWingPathFollower
MODULES += Osd/osdoutout
MODULES += Logging
MODULES += Telemetry
OPTMODULES += ComUsbBridge
SRC += $(FLIGHTLIB)/notification.c
# Include all camera options
CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
# Some diagnostics
CDEFS += -DDIAG_TASKS
# Misc options
CFLAGS += -ffast-math
# List C source files here (C dependencies are automatically generated).
# Use file-extension c for "c-only"-files
ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/discoveryf4bare.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
#ifeq ($(DEBUG), YES)
SRC += $(OPSYSTEM)/dcc_stdio.c
SRC += $(OPSYSTEM)/cm3_fault_handlers.c
#endif
## Misc library functions
SRC += $(FLIGHTLIB)/paths.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
## UAVObjects
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
else
## Test Code
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
# Optional component libraries
include $(FLIGHTLIB)/rscode/library.mk
#include $(FLIGHTLIB)/PyMite/pymite.mk
include $(ROOT_DIR)/make/apps-defs.mk
include $(ROOT_DIR)/make/common-defs.mk

118
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc Normal file
View File

@ -0,0 +1,118 @@
#
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
#
# 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
#
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate
UAVOBJSRCFILENAMES += debuglogsettings
UAVOBJSRCFILENAMES += debuglogcontrol
UAVOBJSRCFILENAMES += debuglogstatus
UAVOBJSRCFILENAMES += debuglogentry
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
UAVOBJSRCFILENAMES += flightplancontrol
UAVOBJSRCFILENAMES += flightplansettings
UAVOBJSRCFILENAMES += flightplanstatus
UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gcsreceiver
UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += oplinkreceiver
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationstatus
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += callbackinfo
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

90
flight/targets/boards/discoveryf4bare/firmware/cm3_fault_handlers.c Normal file
View File

@ -0,0 +1,90 @@
/*
* cm3_fault_handlers.c
*
* Created on: Apr 24, 2011
* Author: msmith
*/
#include <stdint.h>
#include "inc/dcc_stdio.h"
#ifdef STM32F4XX
# include <stm32f4xx.h>
#endif
#ifdef STM32F2XX
# include <stm32f2xx.h>
#endif
#define FAULT_TRAMPOLINE(_vec) \
__attribute__((naked, no_instrument_function)) \
void \
_vec##_Handler(void) \
{ \
__asm(".syntax unified\n" \
"MOVS R0, #4 \n" \
"MOV R1, LR \n" \
"TST R0, R1 \n" \
"BEQ 1f \n" \
"MRS R0, PSP \n" \
"B " #_vec "_Handler2 \n" \
"1: \n" \
"MRS R0, MSP \n" \
"B " #_vec "_Handler2 \n" \
".syntax divided\n"); \
} \
struct hack
struct cm3_frame {
uint32_t r0;
uint32_t r1;
uint32_t r2;
uint32_t r3;
uint32_t r12;
uint32_t lr;
uint32_t pc;
uint32_t psr;
};
FAULT_TRAMPOLINE(HardFault);
FAULT_TRAMPOLINE(BusFault);
FAULT_TRAMPOLINE(UsageFault);
/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */
#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg))
void HardFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nHARD FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(HFSR));
dbg_write_char('\n');
for (;;) {
;
}
}
void BusFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nBUS FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(CFSR));
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(BFAR));
dbg_write_char('\n');
for (;;) {
;
}
}
void UsageFault_Handler2(struct cm3_frame *frame)
{
dbg_write_str("\nUSAGE FAULT");
dbg_write_hex32(frame->pc);
dbg_write_char('\n');
dbg_write_hex32(SCB_REG(CFSR));
dbg_write_char('\n');
for (;;) {
;
}
}

149
flight/targets/boards/discoveryf4bare/firmware/dcc_stdio.c Normal file
View File

@ -0,0 +1,149 @@
/***************************************************************************
* Copyright (C) 2008 by Dominic Rath *
* Dominic.Rath@gmx.de *
* Copyright (C) 2008 by Spencer Oliver *
* spen@spen-soft.co.uk *
* Copyright (C) 2008 by Frederik Kriewtz *
* frederik@kriewitz.eu *
* *
* 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 2 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 "inc/dcc_stdio.h"
#define TARGET_REQ_TRACEMSG 0x00
#define TARGET_REQ_DEBUGMSG_ASCII 0x01
#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8))
#define TARGET_REQ_DEBUGCHAR 0x02
/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel
* DCRDR[7:0] is used by target for status
* DCRDR[15:8] is used by target for write buffer
* DCRDR[23:16] is used for by host for status
* DCRDR[31:24] is used for by host for write buffer */
#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8))
#define BUSY 1
void dbg_write(unsigned long dcc_data)
{
int len = 4;
while (len--) {
/* wait for data ready */
while (NVIC_DBG_DATA_R & BUSY) {
;
}
/* write our data and set write flag - tell host there is data*/
NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY);
dcc_data >>= 8;
}
}
void dbg_trace_point(unsigned long number)
{
dbg_write(TARGET_REQ_TRACEMSG | (number << 8));
}
void dbg_write_u32(const unsigned long *val, long len)
{
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16));
while (len > 0) {
dbg_write(*val);
val++;
len--;
}
}
void dbg_write_u16(const unsigned short *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16));
while (len > 0) {
dcc_data = val[0]
| ((len > 1) ? val[1] << 16 : 0x0000);
dbg_write(dcc_data);
val += 2;
len -= 2;
}
}
void dbg_write_u8(const unsigned char *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16));
while (len > 0) {
dcc_data = val[0]
| ((len > 1) ? val[1] << 8 : 0x00)
| ((len > 2) ? val[2] << 16 : 0x00)
| ((len > 3) ? val[3] << 24 : 0x00);
dbg_write(dcc_data);
val += 4;
len -= 4;
}
}
void dbg_write_str(const char *msg)
{
long len;
unsigned long dcc_data;
for (len = 0; msg[len] && (len < 65536); len++) {
;
}
dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16));
while (len > 0) {
dcc_data = msg[0]
| ((len > 1) ? msg[1] << 8 : 0x00)
| ((len > 2) ? msg[2] << 16 : 0x00)
| ((len > 3) ? msg[3] << 24 : 0x00);
dbg_write(dcc_data);
msg += 4;
len -= 4;
}
}
void dbg_write_char(char msg)
{
dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16));
}
void dbg_write_hex32(const unsigned long val)
{
static const char hextab[] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
};
for (int shift = 28; shift >= 0; shift -= 4) {
dbg_write_char(hextab[(val >> shift) & 0xf]);
}
}

139
flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c Normal file
View File

@ -0,0 +1,139 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @brief These files are the core system files of OpenPilot.
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
* in the main() function of openpilot.c
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @brief This is where the OP firmware starts. Those files also define the compile-time
* options of the firmware.
* @{
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up and runs main OpenPilot tasks.
* @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 "inc/openpilot.h"
#include <uavobjectsinit.h>
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Local Variables */
#define INCLUDE_TEST_TASKS 0
#if INCLUDE_TEST_TASKS
static uint8_t sdcard_available;
#endif
char Buffer[1024];
uint32_t Cache;
/* Function Prototypes */
#if INCLUDE_TEST_TASKS
static void TaskTick(void *pvParameters);
static void TaskTesting(void *pvParameters);
static void TaskHIDTest(void *pvParameters);
static void TaskServos(void *pvParameters);
static void TaskSDCard(void *pvParameters);
#endif
int32_t CONSOLE_Parse(uint8_t port, char c);
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
static void Stack_Change_Weak() __attribute__((weakref("Stack_Change")));
/* Local Variables */
#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority
#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive
static xTaskHandle initTaskHandle;
/* Function Prototypes */
static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
/**
* OpenPilot Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
int main()
{
int result;
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
vPortInitialiseBlocks();
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* For Revolution we use a FreeRTOS task to bring up the system so we can */
/* always rely on FreeRTOS primitive */
result = xTaskCreate(initTask, (const signed char *)"init",
INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY,
&initTaskHandle);
PIOS_Assert(result == pdPASS);
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */
PIOS_LED_Off(PIOS_LED_HEARTBEAT); \
for (;;) { \
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \
PIOS_DELAY_WaitmS(100); \
}
;
return 0;
}
/**
* Initialisation task.
*
* Runs board and module initialisation, then terminates.
*/
void initTask(__attribute__((unused)) void *parameters)
{
/* board driver init */
PIOS_Board_Init();
/* Initialize modules */
MODULE_INITIALISE_ALL;
/* terminate this task */
vTaskDelete(NULL);
}
/**
* @}
* @}
*/

105
flight/targets/boards/discoveryf4bare/firmware/inc/FreeRTOSConfig.h Normal file
View File

@ -0,0 +1,105 @@
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
/**
* @addtogroup PIOS PIOS
* @{
* @addtogroup FreeRTOS FreeRTOS
* @{
*/
/* Notes: We use 5 task priorities */
#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ...
#define configTICK_RATE_HZ ((portTickType)1000)
#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5)
#define configMINIMAL_STACK_SIZE ((unsigned short)512)
#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total
#define configMAX_TASK_NAME_LEN (16)
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 1
#define configUSE_TICK_HOOK 0
#define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 0
#define configUSE_MUTEXES 1
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configQUEUE_REGISTRY_SIZE 10
#define configUSE_TIMERS 1
#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES (2)
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 1
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255
(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */
/* This is the value being used as per the ST library which permits 16
priority values, 0 to 15. This must correspond to the
configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest
NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
/* Enable run time stats collection */
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
/*
* Once we move to CMSIS2 we can at least use:
*
* CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
*
* (still nothing for the DWT registers, surprisingly)
*/
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \
} \
while (0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
/**
* @}
*/
#endif /* FREERTOS_CONFIG_H */

36
flight/targets/boards/discoveryf4bare/firmware/inc/dcc_stdio.h Normal file
View File

@ -0,0 +1,36 @@
/***************************************************************************
* Copyright (C) 2008 by Dominic Rath *
* Dominic.Rath@gmx.de *
* Copyright (C) 2008 by Spencer Oliver *
* spen@spen-soft.co.uk *
* *
* 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 2 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 DCC_STDIO_H
#define DCC_STDIO_H
void dbg_trace_point(unsigned long number);
void dbg_write_u32(const unsigned long *val, long len);
void dbg_write_u16(const unsigned short *val, long len);
void dbg_write_u8(const unsigned char *val, long len);
void dbg_write_str(const char *msg);
void dbg_write_char(char msg);
void dbg_write_hex32(const unsigned long val);
#endif /* DCC_STDIO_H */

52
flight/targets/boards/discoveryf4bare/firmware/inc/openpilot.h Normal file
View File

@ -0,0 +1,52 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file openpilot.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main OpenPilot header.
* @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 OPENPILOT_H
#define OPENPILOT_H
/* PIOS Includes */
#include <pios.h>
/* OpenPilot Libraries */
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);
#endif /* OPENPILOT_H */
/**
* @}
* @}
*/

82
flight/targets/boards/discoveryf4bare/firmware/inc/pios_board_sim.h Normal file
View File

@ -0,0 +1,82 @@
/**
******************************************************************************
*
* @file pios_board.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
* @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_BOARD_H
#define PIOS_BOARD_H
// ------------------------
// PIOS_LED
// ------------------------
#define PIOS_LED_ALARM 0
#define PIOS_LED_HEARTBEAT 1
#define PIOS_LED_NUM 2
// -------------------------
// COM
//
// See also pios_board_posix.c
// -------------------------
// #define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_COM_BUFFER_SIZE 1024
#define PIOS_COM_MAX_DEVS 255
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_spectrum_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG (PIOS_COM_AUX)
#endif
#define PIOS_GCSRCVR_TIMEOUT_MS 200
/**
* glue macros for file IO
* STM32 uses DOSFS for file IO
*/
#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL
#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL
#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length
#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file)
#define PIOS_FCLOSE(file) fclose(file)
#define PIOS_FUNLINK(file) unlink((char *)filename)
#endif /* PIOS_BOARD_H */

178
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h Normal file
View File

@ -0,0 +1,178 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
* @brief PiOS configuration header, the compile time config file for the PIOS.
* Defines which PiOS libraries and features are included in the firmware.
* @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_CONFIG_H
#define PIOS_CONFIG_H
/*
* Below is a complete list of PIOS configurable options.
* Please do not remove or rearrange them. Only comment out
* unused options in the list. See main pios.h header for more
* details.
*/
/* #define PIOS_INCLUDE_DEBUG_CONSOLE */
/* #define DEBUG_LEVEL 0 */
/* #define PIOS_ENABLE_DEBUG_PINS */
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS Callback Scheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
/* PIOS system functions */
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_INITCALL
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_TASK_MONITOR
/* PIOS hardware peripherals */
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_TIM
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_WDG
/* PIOS USB functions */
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_USB_CDC
/* #define PIOS_INCLUDE_USB_RCTX */
/* PIOS sensor interfaces */
/* #define PIOS_INCLUDE_ADXL345 */
/* #define PIOS_INCLUDE_BMA180 */
/* #define PIOS_INCLUDE_L3GD20 */
// #define PIOS_INCLUDE_MPU6000
// #define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
// #define PIOS_INCLUDE_HMC5883
// #define PIOS_HMC5883_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
// #define PIOS_INCLUDE_MS5611
// #define PIOS_INCLUDE_MPXV
// #define PIOS_INCLUDE_ETASV3
/* #define PIOS_INCLUDE_HCSR04 */
/* PIOS receiver drivers */
#define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM
/* #define PIOS_INCLUDE_PPM_FLEXI */
#define PIOS_INCLUDE_DSM
#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_GCSRCVR
// #define PIOS_INCLUDE_OPLINKRCVR
/* PIOS abstract receiver interface */
#define PIOS_INCLUDE_RCVR
/* PIOS common peripherals */
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
/* #define PIOS_INCLUDE_I2C_ESC */
/* #define PIOS_INCLUDE_OVERO */
/* #define PIOS_OVERO_SPI */
/* #define PIOS_INCLUDE_SDCARD */
/* #define LOG_FILENAME "startup.log" */
#define PIOS_INCLUDE_FLASH
#define PIOS_INCLUDE_FLASH_INTERNAL
#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS
#define FLASH_FREERTOS
/* #define PIOS_INCLUDE_FLASH_EEPROM */
/* PIOS radio modules */
// #define PIOS_INCLUDE_RFM22B
// #define PIOS_INCLUDE_RFM22B_COM
// #define PIOS_INCLUDE_RFM22B_RCVR
/* #define PIOS_INCLUDE_PPM_OUT */
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */
/* PIOS misc peripherals */
/* #define PIOS_INCLUDE_VIDEO */
/* #define PIOS_INCLUDE_WAVE */
/* #define PIOS_INCLUDE_UDP */
/* PIOS abstract comms interface with options */
#define PIOS_INCLUDE_COM
/* #define PIOS_INCLUDE_COM_MSG */
/* #define PIOS_INCLUDE_TELEMETRY_RF */
#define PIOS_INCLUDE_COM_TELEM
#define PIOS_INCLUDE_COM_FLEXI
/* #define PIOS_INCLUDE_COM_AUX */
#define PIOS_TELEM_PRIORITY_QUEUE
#define PIOS_INCLUDE_GPS
/* #define PIOS_GPS_MINIMAL */
#define PIOS_INCLUDE_GPS_NMEA_PARSER
#define PIOS_INCLUDE_GPS_UBX_PARSER
#define PIOS_GPS_SETS_HOMELOCATION
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */
/* Performance counters */
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 1000
#define HEAP_LIMIT_CRITICAL 500
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
/* #define PIOS_ACTUATOR_STACK_SIZE 1020 */
/* #define PIOS_MANUAL_STACK_SIZE 800 */
/* #define PIOS_SYSTEM_STACK_SIZE 660 */
/* #define PIOS_STABILIZATION_STACK_SIZE 524 */
/* #define PIOS_TELEM_STACK_SIZE 500 */
/* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */
/* This can't be too high to stop eventdispatcher thread overflowing */
#define PIOS_EVENTDISAPTCHER_QUEUE 10
/* Revolution series */
#define REVOLUTION
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

80
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config_sim.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* @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_CONFIG_POSIX_H
#define PIOS_CONFIG_POSIX_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
// #define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_TCP
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_GCSRCVR
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_RCVR_MAX_DEVS 3
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* COM Module */
#define GPS_BAUDRATE 19200
#define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200
#define TELEM_QUEUE_SIZE 20
#define PIOS_TELEM_STACK_SIZE 2048
/* Stabilization options */
#define PIOS_QUATERNION_STABILIZATION
/* GPS options */
#define PIOS_GPS_SETS_HOMELOCATION
#define HEAP_LIMIT_WARNING 4000
#define HEAP_LIMIT_CRITICAL 1000
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
#define REVOLUTION
#endif /* PIOS_CONFIG_POSIX_H */

46
flight/targets/boards/discoveryf4bare/firmware/inc/pios_usb_board_data.h Normal file
View File

@ -0,0 +1,46 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB 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_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 4
#include <pios_usb_defs.h> /* USB_* macros */
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW)
#define PIOS_USB_BOARD_SN_SUFFIX "+FW"
#endif /* PIOS_USB_BOARD_DATA_H */

940
flight/targets/boards/discoveryf4bare/firmware/pios_board.c Normal file
View File

@ -0,0 +1,940 @@
/**
******************************************************************************
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @brief Defines board specific static initializers for hardware for the revomini board.
*****************************************************************************/
/*
* 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 "inc/openpilot.h"
#include <pios_board_info.h>
#include <uavobjectsinit.h>
#include <hwsettings.h>
#include <manualcontrolsettings.h>
#include <oplinksettings.h>
#include <oplinkstatus.h>
#include <oplinkreceiver.h>
#include <pios_oplinkrcvr_priv.h>
#include <taskinfo.h>
#include <pios_callbackscheduler.h>
/*
* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
* the HW definitions to be const and static to limit their
* scope.
*
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/
#include "../board_hw_defs.c"
/**
* Sensor configurations
*/
#if defined(PIOS_INCLUDE_ADC)
#include "pios_adc_priv.h"
void PIOS_ADC_DMC_irq_handler(void);
void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
struct pios_adc_cfg pios_adc_cfg = {
.adc_dev = ADC1,
.dma = {
.irq = {
.flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4),
.init = {
.NVIC_IRQChannel = DMA2_Stream4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA2_Stream4,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR
},
}
},
.half_flag = DMA_IT_HTIF4,
.full_flag = DMA_IT_TCIF4,
};
void PIOS_ADC_DMC_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_ADC_DMA_Handler();
}
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.irq = {
.init = {
.NVIC_IRQChannel = EXTI9_5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = EXTI_Line7, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Rising,
.EXTI_LineCmd = ENABLE,
},
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
};
#endif /* PIOS_INCLUDE_HMC5883 */
/**
* Configuration for the MS5611 chip
*/
#if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512,
};
#endif /* PIOS_INCLUDE_MS5611 */
/**
* Configuration for the MPU6000 chip
*/
#if defined(PIOS_INCLUDE_MPU6000)
#include "pios_mpu6000.h"
#include "pios_mpu6000_config.h"
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
.vector = PIOS_MPU6000_IRQHandler,
.line = EXTI_Line4,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.irq = {
.init = {
.NVIC_IRQChannel = EXTI4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = EXTI_Line4, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Rising,
.EXTI_LineCmd = ENABLE,
},
},
};
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 12 for 666Hz
.Smpl_rate_div_no_dlp = 11,
// with dlp on output rate is 500Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
};
#endif /* PIOS_INCLUDE_MPU6000 */
/* One slot per selectable receiver group.
* eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS
* NOTE: No slot in this map for NONE.
*/
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
#define PIOS_COM_GPS_RX_BUF_LEN 32
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65
#define PIOS_COM_BRIDGE_RX_BUF_LEN 65
#define PIOS_COM_BRIDGE_TX_BUF_LEN 12
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
#define PIOS_COM_HKOSD_RX_BUF_LEN 22
#define PIOS_COM_HKOSD_TX_BUF_LEN 22
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
uint32_t pios_com_debug_id;
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
uint32_t pios_com_gps_id = 0;
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telem_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
uint32_t pios_com_vcp_id = 0;
#if defined(PIOS_INCLUDE_RFM22B)
uint32_t pios_rfm22b_id = 0;
#endif
uintptr_t pios_uavo_settings_fs_id;
uintptr_t pios_user_fs_id;
/*
* Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only
*/
static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len,
const struct pios_com_driver *com_driver, uint32_t *pios_com_id)
{
uint32_t pios_usart_id;
if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) {
PIOS_Assert(0);
}
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len);
PIOS_Assert(rx_buffer);
if (tx_buf_len != (size_t)-1) { // this is the case for rx/tx ports
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id,
rx_buffer, rx_buf_len,
tx_buffer, tx_buf_len)) {
PIOS_Assert(0);
}
} else { // rx only port
if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id,
rx_buffer, rx_buf_len,
NULL, 0)) {
PIOS_Assert(0);
}
}
}
static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
const struct pios_com_driver *usart_com_driver, enum pios_dsm_proto *proto,
ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind)
{
uint32_t pios_usart_dsm_id;
if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_dsm_id;
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver,
pios_usart_dsm_id, *proto, *bind)) {
PIOS_Assert(0);
}
uint32_t pios_dsm_rcvr_id;
if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id;
}
static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg)
{
/* Set up the receiver port. Later this should be optional */
uint32_t pios_pwm_id;
PIOS_PWM_Init(&pios_pwm_id, pwm_cfg);
uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
}
static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg)
{
uint32_t pios_ppm_id;
PIOS_PPM_Init(&pios_ppm_id, ppm_cfg);
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
}
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
#include <pios_board_info.h>
void PIOS_Board_Init(void)
{
/* Delay system */
PIOS_DELAY_Init();
const struct pios_board_info *bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED)
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 */
#if false
/* Set up the SPI interface to the gyro/acelerometer */
if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
PIOS_DEBUG_Assert(0);
}
/* Set up the SPI interface to the flash and rfm22b */
if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) {
PIOS_DEBUG_Assert(0);
}
#endif
#if defined(PIOS_INCLUDE_FLASH)
/* Connect flash to the appropriate interface and configure it */
uintptr_t flash_id;
// initialize the internal settings storage flash
if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_internal_user_cfg, &pios_internal_flash_driver, flash_id)) {
PIOS_DEBUG_Assert(0);
}
#endif /* if defined(PIOS_INCLUDE_FLASH) */
#if defined(PIOS_INCLUDE_RTC)
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* IAP System Setup */
PIOS_IAP_Init();
// check for safe mode commands from gcs
if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 &&
PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 &&
PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) {
PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
PIOS_IAP_WriteBootCmd(0, 0);
PIOS_IAP_WriteBootCmd(1, 0);
PIOS_IAP_WriteBootCmd(2, 0);
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_Init();
#endif
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
HwSettingsInitialize();
#if defined(PIOS_INCLUDE_RFM22B)
OPLinkSettingsInitialize();
OPLinkStatusInitialize();
#endif /* PIOS_INCLUDE_RFM22B */
/* Initialize the alarms library */
AlarmsInitialize();
/* Set up pulse timers */
PIOS_TIM_InitClock(&tim_1_cfg);
PIOS_TIM_InitClock(&tim_3_cfg);
PIOS_TIM_InitClock(&tim_4_cfg);
PIOS_TIM_InitClock(&tim_5_cfg);
PIOS_TIM_InitClock(&tim_8_cfg);
PIOS_TIM_InitClock(&tim_9_cfg);
PIOS_TIM_InitClock(&tim_10_cfg);
PIOS_TIM_InitClock(&tim_11_cfg);
PIOS_TIM_InitClock(&tim_12_cfg);
uint16_t boot_count = PIOS_IAP_ReadBootCount();
if (boot_count < 3) {
PIOS_IAP_WriteBootCount(++boot_count);
AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
} else {
/* Too many failed boot attempts, force hwsettings to defaults */
HwSettingsSetDefaults(HwSettingsHandle(), 0);
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
}
// PIOS_IAP_Init();
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Flags to determine if various USB interfaces are advertised */
bool usb_hid_present = false;
bool usb_cdc_present = false;
#if defined(PIOS_INCLUDE_USB_CDC)
if (PIOS_USB_DESC_HID_CDC_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
usb_cdc_present = true;
#else
if (PIOS_USB_DESC_HID_ONLY_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
#endif
uint32_t pios_usb_id;
PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
#if defined(PIOS_INCLUDE_USB_CDC)
uint8_t hwsettings_usb_vcpport;
/* Configure the USB VCP port */
HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport);
if (!usb_cdc_present) {
/* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */
hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED;
}
uint32_t pios_usb_cdc_id;
if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
switch (hwsettings_usb_vcpport) {
case HWSETTINGS_USB_VCPPORT_DISABLED:
break;
case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
#if defined(PIOS_INCLUDE_COM)
{
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
break;
case HWSETTINGS_USB_VCPPORT_COMBRIDGE:
#if defined(PIOS_INCLUDE_COM)
{
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
break;
case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE:
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
{
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
NULL, 0,
tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
#endif /* PIOS_INCLUDE_COM */
break;
}
#endif /* PIOS_INCLUDE_USB_CDC */
#if defined(PIOS_INCLUDE_USB_HID)
/* Configure the usb HID port */
uint8_t hwsettings_usb_hidport;
HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport);
if (!usb_hid_present) {
/* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */
hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED;
}
switch (hwsettings_usb_hidport) {
case HWSETTINGS_USB_HIDPORT_DISABLED:
break;
case HWSETTINGS_USB_HIDPORT_USBTELEMETRY:
#if defined(PIOS_INCLUDE_COM)
{
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
break;
}
#endif /* PIOS_INCLUDE_USB_HID */
if (usb_hid_present || usb_cdc_present) {
PIOS_USBHOOK_Activate();
}
#endif /* PIOS_INCLUDE_USB */
/* Configure IO ports */
uint8_t hwsettings_DSMxBind;
HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
/* Configure main USART port */
uint8_t hwsettings_mainport;
HwSettingsRM_MainPortGet(&hwsettings_mainport);
switch (hwsettings_mainport) {
case HWSETTINGS_RM_MAINPORT_DISABLED:
break;
case HWSETTINGS_RM_MAINPORT_TELEMETRY:
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id);
break;
case HWSETTINGS_RM_MAINPORT_GPS:
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
break;
case HWSETTINGS_RM_MAINPORT_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
{
uint32_t pios_usart_sbus_id;
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_sbus_id;
if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
PIOS_Assert(0);
}
uint32_t pios_sbus_rcvr_id;
if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id;
}
#endif
break;
case HWSETTINGS_RM_MAINPORT_DSM2:
case HWSETTINGS_RM_MAINPORT_DSMX10BIT:
case HWSETTINGS_RM_MAINPORT_DSMX11BIT:
{
enum pios_dsm_proto proto;
switch (hwsettings_mainport) {
case HWSETTINGS_RM_MAINPORT_DSM2:
proto = PIOS_DSM_PROTO_DSM2;
break;
case HWSETTINGS_RM_MAINPORT_DSMX10BIT:
proto = PIOS_DSM_PROTO_DSMX10BIT;
break;
case HWSETTINGS_RM_MAINPORT_DSMX11BIT:
proto = PIOS_DSM_PROTO_DSMX11BIT;
break;
default:
PIOS_Assert(0);
break;
}
// Force binding to zero on the main port
hwsettings_DSMxBind = 0;
// TODO: Define the various Channelgroup for Revo dsm inputs and handle here
PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg,
&pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind);
}
break;
case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE:
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
{
PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
}
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
break;
case HWSETTINGS_RM_MAINPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
case HWSETTINGS_RM_MAINPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rm_mainport */
if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) {
GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init);
GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable);
}
/* Configure FlexiPort */
uint8_t hwsettings_flexiport;
HwSettingsRM_FlexiPortGet(&hwsettings_flexiport);
switch (hwsettings_flexiport) {
case HWSETTINGS_RM_FLEXIPORT_DISABLED:
break;
case HWSETTINGS_RM_FLEXIPORT_TELEMETRY:
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id);
break;
case HWSETTINGS_RM_FLEXIPORT_I2C:
#if defined(PIOS_INCLUDE_I2C)
{
if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
break;
case HWSETTINGS_RM_FLEXIPORT_DSM2:
case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT:
case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT:
{
enum pios_dsm_proto proto;
switch (hwsettings_flexiport) {
case HWSETTINGS_RM_FLEXIPORT_DSM2:
proto = PIOS_DSM_PROTO_DSM2;
break;
case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT:
proto = PIOS_DSM_PROTO_DSMX10BIT;
break;
case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT:
proto = PIOS_DSM_PROTO_DSMX11BIT;
break;
default:
PIOS_Assert(0);
break;
}
// TODO: Define the various Channelgroup for Revo dsm inputs and handle here
PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg,
&pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind);
}
break;
case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE:
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
{
PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
}
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
break;
case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE:
PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
break;
case HWSETTINGS_RM_FLEXIPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
} /* hwsettings_rm_flexiport */
/* Initalize the RFM22B radio COM device. */
#if defined(PIOS_INCLUDE_RFM22B)
/* Fetch the OPinkSettings object. */
OPLinkSettingsData oplinkSettings;
OPLinkSettingsGet(&oplinkSettings);
// Initialize out status object.
OPLinkStatusData oplinkStatus;
OPLinkStatusGet(&oplinkStatus);
oplinkStatus.BoardType = bdinfo->board_type;
PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial);
oplinkStatus.BoardRevision = bdinfo->board_rev;
/* Is the radio turned on? */
bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE);
bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE);
bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE);
if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) {
/* Configure the RFM22B device. */
const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
PIOS_Assert(0);
}
/* Configure the radio com interface */
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
// Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.
enum rfm22b_datarate datarate = RFM22_datarate_64000;
switch (oplinkSettings.ComSpeed) {
case OPLINKSETTINGS_COMSPEED_4800:
datarate = RFM22_datarate_9600;
break;
case OPLINKSETTINGS_COMSPEED_9600:
datarate = RFM22_datarate_19200;
break;
case OPLINKSETTINGS_COMSPEED_19200:
datarate = RFM22_datarate_32000;
break;
case OPLINKSETTINGS_COMSPEED_38400:
datarate = RFM22_datarate_64000;
break;
case OPLINKSETTINGS_COMSPEED_57600:
datarate = RFM22_datarate_100000;
break;
case OPLINKSETTINGS_COMSPEED_115200:
datarate = RFM22_datarate_192000;
break;
}
/* Set the radio configuration parameters. */
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
/* Set the PPM callback if we should be receiving PPM. */
if (ppm_mode) {
PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
}
/* Set the modem Tx poer level */
switch (oplinkSettings.MaxRFPower) {
case OPLINKSETTINGS_MAXRFPOWER_125:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
break;
case OPLINKSETTINGS_MAXRFPOWER_16:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
break;
case OPLINKSETTINGS_MAXRFPOWER_316:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
break;
case OPLINKSETTINGS_MAXRFPOWER_63:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
break;
case OPLINKSETTINGS_MAXRFPOWER_126:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
break;
case OPLINKSETTINGS_MAXRFPOWER_25:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
break;
case OPLINKSETTINGS_MAXRFPOWER_50:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
break;
case OPLINKSETTINGS_MAXRFPOWER_100:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
break;
default:
// do nothing
break;
}
/* Reinitialize the modem. */
PIOS_RFM22B_Reinit(pios_rfm22b_id);
} else {
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
}
OPLinkStatusSet(&oplinkStatus);
#endif /* PIOS_INCLUDE_RFM22B */
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM)
const struct pios_servo_cfg *pios_servo_cfg;
// default to servo outputs only
pios_servo_cfg = &pios_servo_cfg_out;
#endif
/* Configure the receiver port*/
uint8_t hwsettings_rcvrport;
HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport);
//
switch (hwsettings_rcvrport) {
case HWSETTINGS_RM_RCVRPORT_DISABLED:
break;
case HWSETTINGS_RM_RCVRPORT_PWM:
#if defined(PIOS_INCLUDE_PWM)
/* Set up the receiver port. Later this should be optional */
PIOS_Board_configure_pwm(&pios_pwm_cfg);
#endif /* PIOS_INCLUDE_PWM */
break;
case HWSETTINGS_RM_RCVRPORT_PPM:
case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS:
case HWSETTINGS_RM_RCVRPORT_PPMPWM:
#if defined(PIOS_INCLUDE_PPM)
if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) {
// configure servo outputs and the remaining 5 inputs as outputs
pios_servo_cfg = &pios_servo_cfg_out_in_ppm;
}
PIOS_Board_configure_ppm(&pios_ppm_cfg);
// enable pwm on the remaining channels
if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) {
PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg);
}
break;
#endif /* PIOS_INCLUDE_PPM */
case HWSETTINGS_RM_RCVRPORT_OUTPUTS:
// configure only the servo outputs
pios_servo_cfg = &pios_servo_cfg_out_in;
break;
}
#if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize();
uint32_t pios_gcsrcvr_id;
PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
uint32_t pios_gcsrcvr_rcvr_id;
if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
#endif /* PIOS_INCLUDE_GCSRCVR */
#if defined(PIOS_INCLUDE_OPLINKRCVR)
{
OPLinkReceiverInitialize();
uint32_t pios_oplinkrcvr_id;
PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id);
uint32_t pios_oplinkrcvr_rcvr_id;
if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id;
}
#endif /* PIOS_INCLUDE_OPLINKRCVR */
#ifndef PIOS_ENABLE_DEBUG_PINS
// pios_servo_cfg points to the correct configuration based on input port settings
PIOS_Servo_Init(pios_servo_cfg);
#else
PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins));
#endif
// Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout
GPIO_InitTypeDef gpioA8 = {
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
.GPIO_Pin = GPIO_Pin_8,
.GPIO_OType = GPIO_OType_OD,
};
GPIO_Init(GPIOA, &gpioA8);
if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_DELAY_WaitmS(50);
#if defined(PIOS_INCLUDE_ADC)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#endif
#if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
}
/**
* @}
* @}
*/

227
flight/targets/boards/discoveryf4bare/firmware/pios_board_sim.c Normal file
View File

@ -0,0 +1,227 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
* @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 "inc/openpilot.h"
#include <pios_com_priv.h>
#include <pios_tcp_priv.h>
#include <pios_udp_priv.h>
#include <pios_rcvr_priv.h>
#include <pios_gcsrcvr_priv.h>
#include <uavobjectsinit.h>
#include <accelssensor.h>
#include <barosensor.h>
#include <gpspositionsensor.h>
#include <gyrosensor.h>
#include <magsensor.h>
#include <manualcontrolsettings.h>
void Stack_Change() {}
void Stack_Change_Weak() {}
const struct pios_tcp_cfg pios_tcp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_udp_cfg pios_udp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_tcp_cfg pios_tcp_gps_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_tcp_cfg pios_tcp_debug_cfg = {
.ip = "0.0.0.0",
.port = 9002,
};
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
const struct pios_tcp_cfg pios_tcp_aux_cfg = {
.ip = "0.0.0.0",
.port = 9003,
};
#endif
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96
/*
* Board specific number of devices.
*/
/*
struct pios_udp_dev pios_udp_devs[] = {
#define PIOS_UDP_TELEM 0
{
.cfg = &pios_udp0_cfg,
},
#define PIOS_UDP_GPS 1
{
.cfg = &pios_udp1_cfg,
},
#define PIOS_UDP_LOCAL 2
{
.cfg = &pios_udp2_cfg,
},
#ifdef PIOS_COM_AUX
#define PIOS_UDP_AUX 3
{
.cfg = &pios_udp3_cfg,
},
#endif
};
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
*/
/*
* COM devices
*/
/*
* Board specific number of devices.
*/
extern const struct pios_com_driver pios_serial_com_driver;
extern const struct pios_com_driver pios_udp_com_driver;
extern const struct pios_com_driver pios_tcp_com_driver;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_spectrum_id;
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void)
{
/* Delay system */
PIOS_DELAY_Init();
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
AccelSensorInitialize();
BaroSensorInitialize();
MagSensorInitialize();
GPSPositionSensorInitialize();
GyroSensorInitialize();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
{
uint32_t pios_tcp_telem_rf_id;
if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0
{
uint32_t pios_udp_telem_rf_id;
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_tcp_gps_id;
if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif /* if defined(PIOS_INCLUDE_COM) */
#if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize();
uint32_t pios_gcsrcvr_id;
PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
uint32_t pios_gcsrcvr_rcvr_id;
if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
#endif /* PIOS_INCLUDE_GCSRCVR */
}
/**
* @}
*/

294
flight/targets/boards/discoveryf4bare/pios_board.h Normal file
View File

@ -0,0 +1,294 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file pios_board.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
* @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_BOARD_H
#define PIOS_BOARD_H
#include <stdbool.h>
// ------------------------
// Timers and Channels Used
// ------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | | | |
TIM2 | --------------- PIOS_DELAY -----------------
TIM3 | | | |
TIM4 | | | |
TIM5 | | | |
TIM6 | | | |
TIM7 | | | |
TIM8 | | | |
------+-----------+-----------+-----------+----------
*/
// ------------------------
// DMA Channels Used
// ------------------------
/* Channel 1 - */
/* Channel 2 - SPI1 RX */
/* Channel 3 - SPI1 TX */
/* Channel 4 - SPI2 RX */
/* Channel 5 - SPI2 TX */
/* Channel 6 - */
/* Channel 7 - */
/* Channel 8 - */
/* Channel 9 - */
/* Channel 10 - */
/* Channel 11 - */
/* Channel 12 - */
// ------------------------
// BOOTLOADER_SETTINGS
// ------------------------
#define BOARD_READABLE true
#define BOARD_WRITABLE true
#define MAX_DEL_RETRYS 3
// ------------------------
// PIOS_LED
// ------------------------
#define PIOS_LED_HEARTBEAT 0
#define PIOS_LED_ALARM 1
#define PIOS_LED_D1 2
#define PIOS_LED_D2 3
#define PIOS_LED_D3 4
#define PIOS_LED_D4 5
// ------------------------
// PIOS_SPI
// See also pios_board.c
// ------------------------
#define PIOS_SPI_MAX_DEVS 3
// ------------------------
// PIOS_WDG
// ------------------------
#define PIOS_WATCHDOG_TIMEOUT 250
#define PIOS_WDG_REGISTER RTC_BKP_DR4
#define PIOS_WDG_ACTUATOR 0x0001
#define PIOS_WDG_STABILIZATION 0x0002
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_SENSORS 0x0010
// ------------------------
// PIOS_I2C
// See also pios_board.c
// ------------------------
#define PIOS_I2C_MAX_DEVS 3
extern uint32_t pios_i2c_mag_pressure_adapter_id;
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id)
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
// -------------------------
// PIOS_USART
//
// See also pios_board.c
// -------------------------
#define PIOS_USART_MAX_DEVS 5
// -------------------------
// PIOS_COM
//
// See also pios_board.c
// -------------------------
#define PIOS_COM_MAX_DEVS 4
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
extern uint32_t pios_com_vcp_id;
extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_OSDHK (pios_com_hkosd_id)
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
extern uint32_t pios_com_debug_id;
#define PIOS_COM_DEBUG (pios_com_debug_id)
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
#if defined(PIOS_INCLUDE_RFM22B)
extern uint32_t pios_rfm22b_id;
extern uint32_t pios_spi_telem_flash_id;
#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id)
#endif /* PIOS_INCLUDE_RFM22B */
// -------------------------
// Packet Handler
// -------------------------
#define RS_ECC_NPARITY 4
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1
extern uint32_t pios_packet_handler;
#define PIOS_PACKET_HANDLER (pios_packet_handler)
// ------------------------
// TELEMETRY
// ------------------------
#define TELEM_QUEUE_SIZE 80
#define PIOS_TELEM_STACK_SIZE 800
// -------------------------
// System Settings
//
// See also System_stm32f4xx.c
// -------------------------
// These macros are deprecated
// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below
// #define PIOS_MASTER_CLOCK
// #define PIOS_PERIPHERAL_CLOCK
// #define PIOS_PERIPHERAL_CLOCK
#define PIOS_SYSCLK 168000000
// Peripherals that belongs to APB1 are:
// DAC |PWR |CAN1,2
// I2C1,2,3 |UART4,5 |USART3,2
// I2S3Ext |SPI3/I2S3 |SPI2/I2S2
// I2S2Ext |IWDG |WWDG
// RTC/BKP reg
// TIM2,3,4,5,6,7,12,13,14
// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2)
// Default APB1 Prescaler = 4
#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2)
// Peripherals belonging to APB2
// SDIO |EXTI |SYSCFG |SPI1
// ADC1,2,3
// USART1,6
// TIM1,8,9,10,11
//
// Default APB2 Prescaler = 2
//
#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK
// -------------------------
// Interrupt Priorities
// -------------------------
#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
// ------------------------
// PIOS_RCVR
// See also pios_board.c
// ------------------------
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_GCSRCVR_TIMEOUT_MS 100
#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200
#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100
// -------------------------
// Receiver PPM input
// -------------------------
#define PIOS_PPM_MAX_DEVS 1
#define PIOS_PPM_NUM_INPUTS 12
// -------------------------
// Receiver PWM input
// -------------------------
#define PIOS_PWM_MAX_DEVS 1
#define PIOS_PWM_NUM_INPUTS 8
// -------------------------
// Receiver SPEKTRUM input
// -------------------------
#define PIOS_SPEKTRUM_MAX_DEVS 2
#define PIOS_SPEKTRUM_NUM_INPUTS 12
// -------------------------
// Receiver S.Bus input
// -------------------------
#define PIOS_SBUS_MAX_DEVS 1
#define PIOS_SBUS_NUM_INPUTS (16 + 2)
// -------------------------
// Receiver DSM input
// -------------------------
#define PIOS_DSM_MAX_DEVS 2
#define PIOS_DSM_NUM_INPUTS 12
// -------------------------
// Servo outputs
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
// --------------------------
// Timer controller settings
// --------------------------
#define PIOS_TIM_MAX_DEVS 6
// -------------------------
// ADC
// PIOS_ADC_PinGet(0) = Current sensor
// PIOS_ADC_PinGet(1) = Voltage sensor
// PIOS_ADC_PinGet(4) = VREF
// PIOS_ADC_PinGet(5) = Temperature sensor
// -------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \
{ NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
/* which is annoying because this then determines the rate at which we generate buffer turnover events */
/* the objective here is to get enough buffer space to support 100Hz averaging rate */
#define PIOS_ADC_NUM_CHANNELS 4
#define PIOS_ADC_MAX_OVERSAMPLING 2
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_USE_TEMP_SENSOR
#define PIOS_ADC_TEMPERATURE_PIN 3
// -------------------------
// USB
// -------------------------
#define PIOS_USB_MAX_DEVS 1
#define PIOS_USB_ENABLED 1 /* Should remove all references to this */
#define PIOS_USB_HID_MAX_DEVS 1
#endif /* PIOS_BOARD_H */
/**
* @}
* @}
*/

100
flight/targets/boards/discoveryf4bare/pios_usb_board_data.c Normal file
View File

@ -0,0 +1,100 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB 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
*/
#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */
#include <pios_sys.h> /* PIOS_SYS_SerialNumberGet */
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
#include <pios_usb_util.h> /* PIOS_USB_UTIL_AsciiToUtf8 */
static const uint8_t usb_product_id[24] = {
sizeof(usb_product_id),
USB_DESC_TYPE_STRING,
'D', 0,
'i', 0,
's', 0,
'c', 0,
'o', 0,
'v', 0,
'e', 0,
'r', 0,
'y', 0,
'F', 0,
'4', 0,
};
static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
};
static const uint8_t usb_vendor_id[28] = {
sizeof(usb_vendor_id),
USB_DESC_TYPE_STRING,
'o', 0,
'p', 0,
'e', 0,
'n', 0,
'p', 0,
'i', 0,
'l', 0,
'o', 0,
't', 0,
'.', 0,
'o', 0,
'r', 0,
'g', 0
};
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
PIOS_SYS_SerialNumberGet((char *)sn);
/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
uint8_t *utf8 = &(usb_serial_number[2]);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1);
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
return 0;
}

2
flight/targets/boards/osd/firmware/Makefile
View File

@ -37,6 +37,8 @@ MODULES += Telemetry
OPTMODULES =
SRC += $(FLIGHTLIB)/notification.c
# Some diagnostics
CDEFS += -DDIAG_TASKS

4
flight/targets/boards/revolution/firmware/Makefile
View File

@ -53,6 +53,8 @@ MODULES += Telemetry
OPTMODULES += ComUsbBridge
SRC += $(FLIGHTLIB)/notification.c
# Include all camera options
CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
@ -96,7 +98,9 @@ endif
# Optional component libraries
include $(FLIGHTLIB)/rscode/library.mk
#include $(FLIGHTLIB)/PyMite/pymite.mk
include $(ROOT_DIR)/make/apps-defs.mk
include $(ROOT_DIR)/make/common-defs.mk

2
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -144,7 +144,7 @@
#define PIOS_GPS_SETS_HOMELOCATION
/* Stabilization options */
// #define PIOS_QUATERNION_STABILIZATION
#define PIOS_QUATERNION_STABILIZATION
/* Performance counters */
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692

2
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -50,6 +50,8 @@ MODULES += Osd/osdoutout
MODULES += Logging
MODULES += Telemetry
SRC += $(FLIGHTLIB)/notification.c
OPTMODULES += ComUsbBridge
# Include all camera options

4
flight/targets/boards/simposix/firmware/Makefile
View File

@ -45,6 +45,8 @@ MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
#MODULES += OveroSync
SRC += $(FLIGHTLIB)/notification.c
# Paths
OPSYSTEM = .
BOARDINC = ..
@ -103,6 +105,8 @@ SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
SRC += $(PIOSCORECOMMON)/pios_debuglog.c
SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c
SRC += $(PIOSCORECOMMON)/pios_deltatime.c
SRC += $(PIOSCORECOMMON)/pios_notify.c
## PIOS Hardware
include $(PIOS)/posix/library.mk

92
ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
View File

@ -27,10 +27,10 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="7.4629556"
inkscape:cx="32.393349"
inkscape:cy="37.450437"
inkscape:current-layer="foreground"
inkscape:zoom="29.851822"
inkscape:cx="82.604436"
inkscape:cy="54.252252"
inkscape:current-layer="background"
id="namedview3608"
showgrid="true"
inkscape:window-width="1366"
@ -1002,12 +1002,13 @@
inkscape:label="#rect4550-8-1-4-21-5-13" />
<rect
style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
id="rect4550-8-1-4-21-5-8"
id="Magnetometer"
width="18.966711"
height="10.202584"
x="570.72327"
y="361.65591"
ry="1" />
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-8" />
<rect
style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
id="Battery"
@ -1123,6 +1124,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer42"
inkscape:label="Magnetometer-OK"
style="display:none">
<rect
style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-OK"
width="18.966711"
height="10.202584"
x="73.057671"
y="17.391815"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer10"
@ -1399,7 +1415,7 @@
inkscape:groupmode="layer"
id="g5044"
inkscape:label="Airspeed-Warning"
style="display:inline">
style="display:none">
<rect
style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
id="Airspeed-Warning"
@ -1410,6 +1426,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer41"
inkscape:label="Magnetometer-Warning"
style="display:none">
<rect
style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-Warning"
width="18.966711"
height="10.202584"
x="73.091171"
y="17.425316"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="g5047"
@ -1686,7 +1717,7 @@
inkscape:groupmode="layer"
id="g5420"
inkscape:label="Airspeed-Critical"
style="display:inline">
style="display:none">
<rect
style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
id="Airspeed-Critical"
@ -1697,6 +1728,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer40"
inkscape:label="Magnetometer-Critical"
style="display:none">
<rect
style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-Critical"
width="18.966711"
height="10.202584"
x="73.02417"
y="17.391815"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="g5423"
@ -2055,6 +2101,28 @@
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer4"
inkscape:label="Magnetometer-Error"
style="display:none">
<g
style="display:inline"
id="Magnetometer-Error"
inkscape:label="#g3398"
transform="matrix(1.0309004,0,0,1,20.190366,0.03602308)">
<path
inkscape:connector-curvature="0"
id="path6233-9-9-79-9"
d="m 51.59088,18.071902 18.268111,8.770366"
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19476616;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
<path
inkscape:connector-curvature="0"
id="path6233-9-9-4-3-4"
d="M 51.580734,26.818576 69.902677,18.119165"
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer2"
@ -2450,15 +2518,15 @@
<text
xml:space="preserve"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
x="93.336571"
x="94.605125"
y="20.116049"
id="text5330-7-7-8"
sodipodi:linespacing="125%"
transform="scale(0.83127393,1.2029729)"><tspan
sodipodi:role="line"
id="tspan4295"
x="93.336571"
y="20.116049">BARO</tspan></text>
id="tspan3835"
x="94.605125"
y="20.116049">MAG</tspan></text>
<text
xml:space="preserve"
style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 103 KiB

After

Width:  |  Height:  |  Size: 105 KiB

16
ground/openpilotgcs/src/plugins/config/calibration/calibrationuiutils.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef CALIBRATIONUIUTILS_H
#define CALIBRATIONUIUTILS_H
#define CALIBRATION_HELPER_IMAGE_NED QStringLiteral("ned")
#define CALIBRATION_HELPER_IMAGE_DWN QStringLiteral("dwn")
#define CALIBRATION_HELPER_IMAGE_ENU QStringLiteral("enu")
#define CALIBRATION_HELPER_IMAGE_SUW QStringLiteral("suw")
#define CALIBRATION_HELPER_IMAGE_SWD QStringLiteral("swd")
#define CALIBRATION_HELPER_IMAGE_USE QStringLiteral("use")
#define CALIBRATION_HELPER_IMAGE_WDS QStringLiteral("wds")
#define CALIBRATION_HELPER_IMAGE_EMPTY QStringLiteral("empty")
#define CALIBRATION_HELPER_BOARD_PREFIX QStringLiteral("board-")
#define CALIBRATION_HELPER_PLANE_PREFIX QStringLiteral("plane-")
#endif // CALIBRATIONUIUTILS_H

213
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp
View File

@ -42,13 +42,16 @@ float CalibrationUtils::ComputeSigma(Eigen::VectorXf *samplesY)
* The following ellipsoid calibration code is based on RazorImu calibration samples that can be found here:
* https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration
*/
bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result)
bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
float nominalRange,
EllipsoidCalibrationResult *result,
bool fitAlongXYZ)
{
Eigen::VectorXf radii;
Eigen::Vector3f center;
Eigen::MatrixXf evecs;
EllipsoidFit(samplesX, samplesY, samplesZ, &center, &radii, &evecs);
EllipsoidFit(samplesX, samplesY, samplesZ, &center, &radii, &evecs, fitAlongXYZ);
result->Scale.setZero();
@ -280,26 +283,36 @@ void CalibrationUtils::ComputePoly(VectorXf *samplesX, Eigen::VectorXf *polynomi
*/
void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
Eigen::Vector3f *center,
Eigen::VectorXf *radii,
Eigen::MatrixXf *evecs)
Eigen::MatrixXf *evecs,
bool fitAlongXYZ)
{
int numSamples = (*samplesX).rows();
Eigen::MatrixXf D(numSamples, 9);
D.setZero();
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
D.col(6) = 2 * (*samplesX);
D.col(7) = 2 * (*samplesY);
D.col(8) = 2 * (*samplesZ);
Eigen::MatrixXf D;
if (!fitAlongXYZ) {
D.setZero(numSamples, 9);
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
D.col(6) = 2 * (*samplesX);
D.col(7) = 2 * (*samplesY);
D.col(8) = 2 * (*samplesZ);
} else {
D.setZero(numSamples, 6);
D.setZero();
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = 2 * (*samplesX);
D.col(4) = 2 * (*samplesY);
D.col(5) = 2 * (*samplesZ);
}
Eigen::VectorXf ones(numSamples);
ones.setOnes(numSamples);
@ -307,25 +320,163 @@ void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *
Eigen::MatrixXf dt2 = (D.transpose() * ones);
Eigen::VectorXf v = dt1.inverse() * dt2;
Eigen::Matrix4f A;
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
v.coeff(6), v.coeff(7), v.coeff(8), -1;
if (!fitAlongXYZ) {
Eigen::Matrix4f A;
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
v.coeff(6), v.coeff(7), v.coeff(8), -1;
(*center) = -1 * A.block(0, 0, 3, 3).inverse() * v.segment(6, 3);
(*center) = -1 * A.block(0, 0, 3, 3).inverse() * v.segment(6, 3);
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
t.block(3, 0, 1, 3) = center->transpose();
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
t.block(3, 0, 1, 3) = center->transpose();
Eigen::Matrix4f r = t * A * t.transpose();
Eigen::Matrix4f r = t * A * t.transpose();
Eigen::Matrix3f tmp2 = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3);
Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
Eigen::VectorXf evals = es.eigenvalues().real();
(*evecs) = es.eigenvectors().real();
radii->setZero(3);
Eigen::Matrix3f tmp2 = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3);
Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
Eigen::VectorXf evals = es.eigenvalues().real();
(*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt();
(*evecs) = es.eigenvectors().real();
radii->setZero(3);
(*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt();
} else {
Eigen::VectorXf v1(9);
v1 << v.coeff(0), v.coeff(1), v.coeff(2), 0.0f, 0.0f, 0.0f, v.coeff(3), v.coeff(4), v.coeff(5);
(*center) = (-1) * v1.segment(6, 3).cwiseProduct(v1.segment(0, 3).cwiseInverse());
float gam = 1 + (v1.coeff(6) * v1.coeff(6) / v1.coeff(0) +
v1.coeff(7) * v1.coeff(7) / v1.coeff(1) +
v1.coeff(8) * v1.coeff(8) / v1.coeff(2));
radii->setZero(3);
(*radii) = (v1.segment(0, 3).cwiseInverse() * gam).cwiseSqrt();
evecs->setIdentity(3, 3);
}
}
int CalibrationUtils::SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
{
int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i = 0; i < 5; i++) {
A[i][0] = 2.0 * (x[i + 1] - x[i]);
A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
A[i][2] = 2.0 * (y[i + 1] - y[i]);
A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
A[i][4] = 2.0 * (z[i + 1] - z[i]);
f[i] = x[i] * x[i] - x[i + 1] * x[i + 1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if (!LinearEquationsSolve(5, (double *)A, f, c)) {
return 0;
}
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
S[0] = Sx;
b[0] = Sx * c[0];
S[1] = sqrt(c[1] * Sx * Sx);
b[1] = c[2] * Sx * Sx / S[1];
S[2] = sqrt(c[3] * Sx * Sx);
b[2] = c[4] * Sx * Sx / S[2];
return 1;
}
int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
{
double fMaxElem;
double fAcc;
int i, j, k, m;
for (k = 0; k < (nDim - 1); k++) { // base row of matrix
// search of line with max element
fMaxElem = fabs(pfMatr[k * nDim + k]);
m = k;
for (i = k + 1; i < nDim; i++) {
if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
fMaxElem = pfMatr[i * nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if (m != k) {
for (i = k; i < nDim; i++) {
fAcc = pfMatr[k * nDim + i];
pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
pfMatr[m * nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if (pfMatr[k * nDim + k] == 0.) {
return 0; // needs improvement !!!
}
// triangulation of matrix with coefficients
for (j = (k + 1); j < nDim; j++) { // current row of matrix
fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
for (i = k; i < nDim; i++) {
pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
}
pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
}
}
for (k = (nDim - 1); k >= 0; k--) {
pfSolution[k] = pfVect[k];
for (i = (k + 1); i < nDim; i++) {
pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
}
return 1;
}
double CalibrationUtils::listMean(QList<double> list)
{
double accum = 0;
for (int i = 0; i < list.size(); i++) {
accum += list[i];
}
return accum / list.size();
}
double CalibrationUtils::listVar(QList<double> list)
{
double mean_accum = 0;
double var_accum = 0;
double mean;
for (int i = 0; i < list.size(); i++) {
mean_accum += list[i];
}
mean = mean_accum / list.size();
for (int i = 0; i < list.size(); i++) {
var_accum += (list[i] - mean) * (list[i] - mean);
}
// Use unbiased estimator
return var_accum / (list.size() - 1);
}
}

14
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h
View File

@ -32,7 +32,7 @@
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <QList>
namespace OpenPilot {
class CalibrationUtils {
public:
@ -41,17 +41,25 @@ public:
Eigen::Vector3f Scale;
Eigen::Vector3f Bias;
};
static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result);
static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
float nominalRange,
EllipsoidCalibrationResult *result,
bool fitAlongXYZ);
static bool PolynomialCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY);
static float ComputeSigma(Eigen::VectorXf *samplesY);
static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
static double listMean(QList<double> list);
static double listVar(QList<double> list);
private:
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
Eigen::Vector3f *center,
Eigen::VectorXf *radii,
Eigen::MatrixXf *evecs);
Eigen::MatrixXf *evecs, bool fitAlongXYZ);
static int LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution);
};
}
#endif // CALIBRATIONUTILS_H

216
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp Normal file
View File

@ -0,0 +1,216 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include <gyrostate.h>
#include <gyrosensor.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include "calibration/gyrobiascalibrationmodel.h"
#include "calibration/calibrationutils.h"
#include "calibration/calibrationuiutils.h"
static const int LEVEL_SAMPLES = 100;
#include "gyrobiascalibrationmodel.h"
namespace OpenPilot {
GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) :
QObject(parent),
collectingData(false)
{}
/******* gyro bias zero ******/
void GyroBiasCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
storeAndClearBoardRotation();
disableAllCalibrations();
progressChanged(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
displayInstructions(tr("Calibrating the gyroscopes. Keep the copter/plane steady..."), true);
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
gyro_state_accum_x.clear();
gyro_state_accum_y.clear();
gyro_state_accum_z.clear();
UAVObject::Metadata metadata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
metadata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(metadata, UAVObject::UPDATEMODE_PERIODIC);
metadata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(metadata);
UAVObject::Metadata gyroSensorMetadata;
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
initialGyroSensorMdata = gyroSensor->getMetadata();
gyroSensorMetadata = initialGyroSensorMdata;
UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC);
gyroSensorMetadata.flightTelemetryUpdatePeriod = 100;
gyroSensor->setMetadata(gyroSensorMetadata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
/**
Updates the gyro bias raw values
*/
void GyroBiasCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_state_accum_x.append(gyroStateData.x);
gyro_state_accum_y.append(gyroStateData.y);
gyro_state_accum_z.append(gyroStateData.z);
break;
}
case GyroSensor::OBJID:
{
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
GyroSensor::DataFields gyroSensorData = gyroSensor->getData();
gyro_accum_x.append(gyroSensorData.x);
gyro_accum_y.append(gyroSensorData.y);
gyro_accum_z.append(gyroSensorData.z);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)gyro_state_accum_x.size() / (double)LEVEL_SAMPLES;
double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES;
progressChanged(((p1 > p2) ? p1 : p2) * 100);
if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) &&
collectingData == true) {
collectingData = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
Q_ASSERT(gyroState);
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
enableAllCalibrations();
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update biases based on collected data
// check whether the board does supports gyroSensor(updates were received)
if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) {
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z);
} else {
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
}
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
gyroState->setMetadata(initialGyroStateMdata);
gyroSensor->setMetadata(initialGyroSensorMdata);
displayInstructions(tr("Gyroscope calibration computed succesfully."), false);
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
// Recall saved board rotation
recallBoardRotation();
}
}
UAVObjectManager *GyroBiasCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

72
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 GYROBIASCALIBRATIONMODEL_H
#define GYROBIASCALIBRATIONMODEL_H
#include <QObject>
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
namespace OpenPilot {
class GyroBiasCalibrationModel : public QObject {
Q_OBJECT
public:
explicit GyroBiasCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void storeAndClearBoardRotation();
void recallBoardRotation();
void progressChanged(int value);
public slots:
// Slots for gyro bias zero
void start();
private slots:
void getSample(UAVObject *obj);
private:
QMutex sensorsUpdateLock;
bool collectingData;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
QList<double> gyro_state_accum_x;
QList<double> gyro_state_accum_y;
QList<double> gyro_state_accum_z;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialGyroSensorMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // GYROBIASCALIBRATIONMODEL_H

178
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp Normal file
View File

@ -0,0 +1,178 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 "levelcalibrationmodel.h"
#include <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include "calibration/calibrationuiutils.h"
static const int LEVEL_SAMPLES = 100;
namespace OpenPilot {
LevelCalibrationModel::LevelCalibrationModel(QObject *parent) :
QObject(parent)
{}
/******* Level calibration *******/
/**
* Starts an accelerometer bias calibration.
*/
void LevelCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
disableAllCalibrations();
progressChanged(0);
rot_data_pitch = 0;
rot_data_roll = 0;
UAVObject::Metadata mdata;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
initialAttitudeStateMdata = attitudeState->getMetadata();
mdata = initialAttitudeStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
attitudeState->setMetadata(mdata);
/* Show instructions and enable controls */
displayInstructions(tr("Place horizontally and click Save Position button..."), true);
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
disableAllCalibrations();
savePositionEnabledChanged(true);
position = 0;
}
void LevelCalibrationModel::savePosition()
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
savePositionEnabledChanged(false);
rot_accum_pitch.clear();
rot_accum_roll.clear();
collectingData = true;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
displayInstructions(tr("Hold..."), false);
}
/**
Updates the accel bias raw values
*/
void LevelCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case AttitudeState::OBJID:
{
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
AttitudeState::DataFields attitudeStateData = attitudeState->getData();
rot_accum_roll.append(attitudeStateData.Roll);
rot_accum_pitch.append(attitudeStateData.Pitch);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES;
progressChanged(p1 * 100);
if (rot_accum_roll.size() >= LEVEL_SAMPLES &&
collectingData == true) {
collectingData = false;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
position++;
switch (position) {
case 1:
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and click Save Position button..."), false);
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD);
disableAllCalibrations();
savePositionEnabledChanged(true);
break;
case 2:
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_pitch /= 2;
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
rot_data_roll /= 2;
attitudeState->setMetadata(initialAttitudeStateMdata);
compute();
enableAllCalibrations();
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
displayInstructions(tr("Board leveling computed successfully."), false);
break;
}
}
}
void LevelCalibrationModel::compute()
{
enableAllCalibrations();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
// Update the biases based on collected data
// "rotate" the board in the opposite direction as the calculated offset
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
}
UAVObjectManager *LevelCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

74
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h Normal file
View File

@ -0,0 +1,74 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 LEVELCALIBRATIONMODEL_H
#define LEVELCALIBRATIONMODEL_H
#include <QObject>
#include <QMutex>
#include <QList>
#include "calibration/calibrationutils.h"
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
namespace OpenPilot {
class LevelCalibrationModel : public QObject {
Q_OBJECT
public:
explicit LevelCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void savePositionEnabledChanged(bool state);
void progressChanged(int value);
public slots:
// Slots for calibrating the mags
void start();
void savePosition();
private slots:
void getSample(UAVObject *obj);
void compute();
private:
QMutex sensorsUpdateLock;
int position;
bool collectingData;
QList<double> rot_accum_roll;
QList<double> rot_accum_pitch;
double rot_data_roll;
double rot_data_pitch;
UAVObject::Metadata initialAttitudeStateMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // LEVELCALIBRATIONMODEL_H

512
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp Normal file
View File

@ -0,0 +1,512 @@
/**
******************************************************************************
*
* @file sixpointcalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @brief Six point calibration for Magnetometer and Accelerometer
* @see The GNU Public License (GPL) Version 3
* @defgroup
* @{
*
*****************************************************************************/
/*
* 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 "sixpointcalibrationmodel.h"
#include <QThread>
#include "extensionsystem/pluginmanager.h"
#include <QMessageBox>
#include "math.h"
#include "calibration/calibrationuiutils.h"
#include "QDebug"
#define POINT_SAMPLE_SIZE 50
#define GRAVITY 9.81f
#define sign(x) ((x < 0) ? -1 : 1)
#define FITTING_USING_CONTINOUS_ACQUISITION
namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent),
calibrationStepsMag(),
calibrationStepsAccelOnly(),
currentSteps(0),
position(-1),
calibratingMag(false),
calibratingAccel(false),
collectingData(false)
{
calibrationStepsMag.clear();
calibrationStepsMag
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally, nose pointing north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down, right side west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down, nose west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down, nose east and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up, left side north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and click Save Position button..."));
calibrationStepsAccelOnly.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down and click Save Position button..."));
}
/********** Six point calibration **************/
void SixPointCalibrationModel::magStart()
{
start(false, true);
}
void SixPointCalibrationModel::accelStart()
{
start(true, false);
}
/**
* Called by the "Start" button. Sets up the meta data and enables the
* buttons to perform six point calibration of the magnetometer (optionally
* accel) to compute the scale and bias of this sensor based on the current
* home location magnetic strength.
*/
void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
{
calibratingAccel = calibrateAccel;
calibratingMag = calibrateMag;
// Store and reset board rotation before calibration starts
storeAndClearBoardRotation();
if (calibrateMag) {
currentSteps = &calibrationStepsMag;
} else {
currentSteps = &calibrationStepsAccelOnly;
}
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
savedSettings.revoCalibration = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
savedSettings.accelGyroSettings = accelGyroSettings->getData();
// check if Homelocation is set
if (!homeLocationData.Set) {
QMessageBox msgBox;
msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.setIcon(QMessageBox::Information);
msgBox.exec();
return;
}
// Calibration accel
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
// Calibration mag
// Reset the transformation matrix to identity
for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) {
revoCalibrationData.mag_transform[i] = 0;
}
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1;
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1;
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 1;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
// Disable adaptive mag nulling
initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
revoCalibrationData.MagBiasNullingRate = 0;
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
QThread::usleep(100000);
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
UAVObject::Metadata mdata;
/* Need to get as many accel updates as possible */
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
initialAccelStateMdata = accelState->getMetadata();
if (calibrateAccel) {
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accelState->setMetadata(mdata);
}
/* Need to get as many mag updates as possible */
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
initialMagStateMdata = mag->getMetadata();
if (calibrateMag) {
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
}
/* Show instructions and enable controls */
displayInstructions((*currentSteps)[0].instructions, true);
showHelp((*currentSteps)[0].visualHelp);
disableAllCalibrations();
savePositionEnabledChanged(true);
position = 0;
mag_fit_x.clear();
mag_fit_y.clear();
mag_fit_z.clear();
}
/**
* Saves the data from the aircraft in one of six positions.
* This is called when they click "save position" and starts
* averaging data for this position.
*/
void SixPointCalibrationModel::savePositionData()
{
QMutexLocker lock(&sensorsUpdateLock);
savePositionEnabledChanged(false);
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
collectingData = true;
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
if (calibratingMag) {
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
// Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
if (!position) {
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
if (calibratingAccel) {
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
displayInstructions(tr("Hold..."), false);
}
/**
* Grab a sample of mag (optionally accel) data while in this position and
* store it for averaging. When sufficient points are collected advance
* to the next position (give message to user) or compute the scale and bias
*/
void SixPointCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
// This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) {
if (obj->getObjID() == AccelState::OBJID) {
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
} else if (obj->getObjID() == MagState::OBJID) {
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
MagState::DataFields magData = mag->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
#ifndef FITTING_USING_CONTINOUS_ACQUISITION
mag_fit_x.append(magData.x);
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION
} else {
Q_ASSERT(0);
}
}
if ((!calibratingAccel || (accel_accum_x.size() >= POINT_SAMPLE_SIZE)) &&
(!calibratingMag || (mag_accum_x.size() >= POINT_SAMPLE_SIZE / 10)) &&
(collectingData == true)) {
collectingData = false;
savePositionEnabledChanged(true);
// Store the mean for this position for the accel
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
if (calibratingAccel) {
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
accel_data_x[position] = CalibrationUtils::listMean(accel_accum_x);
accel_data_y[position] = CalibrationUtils::listMean(accel_accum_y);
accel_data_z[position] = CalibrationUtils::listMean(accel_accum_z);
}
// Store the mean for this position for the mag
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
if (calibratingMag) {
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x);
mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
}
position = (position + 1) % 6;
if (position != 0) {
displayInstructions((*currentSteps)[position].instructions, false);
showHelp((*currentSteps)[position].visualHelp);
} else {
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
if (calibratingMag) {
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
compute(calibratingMag, calibratingAccel);
savePositionEnabledChanged(false);
enableAllCalibrations();
showHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
/* Cleanup original settings */
accelState->setMetadata(initialAccelStateMdata);
mag->setMetadata(initialMagStateMdata);
// Recall saved board rotation
recallBoardRotation();
}
}
}
void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
if (obj->getObjID() == MagState::OBJID) {
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
MagState::DataFields magData = mag->getData();
mag_fit_x.append(magData.x);
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
}
}
/**
* Computes the scale and bias for the magnetomer and (compile option)
* for the accel once all the data has been collected in 6 positions.
*/
void SixPointCalibrationModel::compute(bool mag, bool accel)
{
double S[3], b[3];
double Be_length;
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation);
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
// Calibration accel
if (accel) {
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
}
// Calibration mag
if (mag) {
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
int vectSize = mag_fit_x.count();
Eigen::VectorXf samples_x(vectSize);
Eigen::VectorXf samples_y(vectSize);
Eigen::VectorXf samples_z(vectSize);
for (int i = 0; i < vectSize; i++) {
samples_x(i) = mag_fit_x[i];
samples_y(i) = mag_fit_y[i];
samples_z(i) = mag_fit_z[i];
}
OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Fit";
qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Six Point";
qDebug() << "scale(" << S[0] << ", " << S[1] << ", " << S[2] << ")";
qDebug() << "bias(" << -sign(S[0]) * b[0] << ", " << -sign(S[1]) * b[1] << ", " << -sign(S[2]) * b[2] << ")";
qDebug() << "-----------------------------------";
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
}
// Restore the previous setting
revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
bool good_calibration = true;
// Check the mag calibration is good
if (mag) {
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
}
// Check the accel calibration is good
if (accel) {
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
}
if (good_calibration) {
if (mag) {
revoCalibration->setData(revoCalibrationData);
} else {
revoCalibration->setData(savedSettings.revoCalibration);
}
if (accel) {
accelGyroSettings->setData(accelGyroSettingsData);
} else {
accelGyroSettings->setData(savedSettings.accelGyroSettings);
}
displayInstructions(tr("Sensor scale and bias computed succesfully."), true);
} else {
displayInstructions(tr("Bad calibration. Please review the instructions and repeat."), true);
}
position = -1; // set to run again
}
UAVObjectManager *SixPointCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
void SixPointCalibrationModel::showHelp(QString image)
{
if (image == CALIBRATION_HELPER_IMAGE_EMPTY) {
displayVisualHelp(image);
} else {
if (calibratingAccel) {
displayVisualHelp(CALIBRATION_HELPER_BOARD_PREFIX + image);
} else {
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + image);
}
}
}
}

116
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h Normal file
View File

@ -0,0 +1,116 @@
/**
******************************************************************************
*
* @file sixpointcalibrationmodel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @brief Six point calibration for Magnetometer and Accelerometer
* @see The GNU Public License (GPL) Version 3
* @defgroup
* @{
*
*****************************************************************************/
/*
* 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 SIXPOINTCALIBRATIONMODEL_H
#define SIXPOINTCALIBRATIONMODEL_H
#include <QMutex>
#include <QObject>
#include <QList>
#include "calibration/calibrationutils.h"
#include <QString>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
namespace OpenPilot {
class SixPointCalibrationModel : public QObject {
Q_OBJECT
class CalibrationStep {
public:
CalibrationStep(QString newVisualHelp, QString newInstructions)
{
visualHelp = newVisualHelp;
instructions = newInstructions;
}
QString visualHelp;
QString instructions;
};
typedef struct {
RevoCalibration::DataFields revoCalibration;
AccelGyroSettings::DataFields accelGyroSettings;
} SavedSettings;
public:
explicit SixPointCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void storeAndClearBoardRotation();
void recallBoardRotation();
void savePositionEnabledChanged(bool state);
public slots:
// Slots for calibrating the mags
void magStart();
void accelStart();
void savePositionData();
private slots:
void getSample(UAVObject *obj);
void continouslyGetMagSamples(UAVObject *obj);
private:
void start(bool calibrateAccel, bool calibrateMag);
UAVObjectManager *getObjectManager();
QList<CalibrationStep> calibrationStepsMag;
QList<CalibrationStep> calibrationStepsAccelOnly;
QList<CalibrationStep> *currentSteps;
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialMagStateMdata;
float initialMagCorrectionRate;
SavedSettings savedSettings;
int position;
bool calibratingMag;
bool calibratingAccel;
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
double mag_data_x[6], mag_data_y[6], mag_data_z[6];
QMutex sensorsUpdateLock;
// ! Computes the scale and bias of the mag based on collected data
void compute(bool mag, bool accel);
bool collectingData;
QList<double> accel_accum_x;
QList<double> accel_accum_y;
QList<double> accel_accum_z;
QList<double> mag_accum_x;
QList<double> mag_accum_y;
QList<double> mag_accum_z;
QList<float> mag_fit_x;
QList<float> mag_fit_y;
QList<float> mag_fit_z;
void showHelp(QString image);
};
}
#endif // SIXPOINTCALIBRATIONMODEL_H

11
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -51,7 +51,11 @@ HEADERS += configplugin.h \
calibration/thermal/boardsetuptransition.h \
calibration/thermal/dataacquisitiontransition.h \
calibration/thermal/settingshandlingtransitions.h \
calibration/thermal/compensationcalculationtransition.h
calibration/thermal/compensationcalculationtransition.h \
calibration/sixpointcalibrationmodel.h \
calibration/levelcalibrationmodel.h \
calibration/gyrobiascalibrationmodel.h \
calibration/calibrationuiutils.h
SOURCES += configplugin.cpp \
configgadgetwidget.cpp \
@ -86,7 +90,10 @@ SOURCES += configplugin.cpp \
calibration/wizardmodel.cpp \
calibration/thermal/thermalcalibration.cpp \
calibration/thermal/thermalcalibrationhelper.cpp \
calibration/thermal/thermalcalibrationmodel.cpp
calibration/thermal/thermalcalibrationmodel.cpp \
calibration/sixpointcalibrationmodel.cpp \
calibration/levelcalibrationmodel.cpp \
calibration/gyrobiascalibrationmodel.cpp
FORMS += airframe.ui \
airframe_ccpm.ui \

14
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -38,7 +38,7 @@
#include "gyrostate.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
#include <calibration/calibrationutils.h>
ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
ConfigTaskWidget(parent),
ui(new Ui_ccattitude)
@ -114,13 +114,13 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
float x_bias = listMean(x_accum);
float y_bias = listMean(y_accum);
float z_bias = (listMean(z_accum) + 9.81);
float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum);
float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum);
float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81;
float x_gyro_bias = listMean(x_gyro_accum);
float y_gyro_bias = listMean(y_gyro_accum);
float z_gyro_bias = listMean(z_gyro_accum);
float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum);
float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum);
float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum);
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);

16
ground/openpilotgcs/src/plugins/config/configgadget.qrc
View File

@ -2,7 +2,6 @@
<qresource prefix="/configgadget">
<file>images/help2.png</file>
<file>images/ahrs-calib.svg</file>
<file>images/paper-plane.svg</file>
<file>images/multirotor-shapes.svg</file>
<file>images/ccpm_setup.svg</file>
<file>images/PipXtreme.png</file>
@ -31,5 +30,20 @@
<file>images/pipx-normal.png</file>
<file>images/revolution_top.png</file>
<file>calibration/WizardStepIndicator.qml</file>
<file>images/calibration/board-dwn.png</file>
<file>images/calibration/board-enu.png</file>
<file>images/calibration/plane-dwn.png</file>
<file>images/calibration/plane-enu.png</file>
<file>images/calibration/plane-ned.png</file>
<file>images/calibration/plane-suw.png</file>
<file>images/calibration/plane-use.png</file>
<file>images/calibration/plane-wds.png</file>
<file>images/calibration/board-ned.png</file>
<file>images/calibration/board-suw.png</file>
<file>images/calibration/board-use.png</file>
<file>images/calibration/board-wds.png</file>
<file>images/calibration/empty.png</file>
<file>images/calibration/plane-swd.png</file>
<file>images/calibration/board-swd.png</file>
</qresource>
</RCC>

1045
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -40,25 +40,23 @@
#include <iostream>
#include <QDesktopServices>
#include <QUrl>
#include <attitudestate.h>
#include <attitudesettings.h>
#include <ekfconfiguration.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <gyrostate.h>
#include <magstate.h>
#define GRAVITY 9.81f
#include "assertions.h"
#include "calibration.h"
#include "calibration/calibrationutils.h"
#define sign(x) ((x < 0) ? -1 : 1)
// Uncomment this to enable 6 point calibration on the accels
#define SIX_POINT_CAL_ACCEL
#define NOISE_SAMPLES 50
const double ConfigRevoWidget::maxVarValue = 0.1;
// *****************
@ -75,146 +73,18 @@ public:
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent),
m_ui(new Ui_RevoSensorsWidget()),
collectingData(false),
position(-1),
isBoardRotationStored(false)
{
m_ui->setupUi(this);
// Initialization of the Paper plane widget
m_ui->sixPointsHelp->setScene(new QGraphicsScene(this));
paperplane = new QGraphicsSvgItem();
paperplane->setSharedRenderer(new QSvgRenderer());
paperplane->renderer()->load(QString(":/configgadget/images/paper-plane.svg"));
paperplane->setElementId("plane-horizontal");
m_ui->sixPointsHelp->scene()->addItem(paperplane);
m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
// Initialization of the Revo sensor noise bargraph graph
m_ui->sensorsBargraph->setScene(new QGraphicsScene(this));
QSvgRenderer *renderer = new QSvgRenderer();
sensorsBargraph = new QGraphicsSvgItem();
renderer->load(QString(":/configgadget/images/ahrs-calib.svg"));
sensorsBargraph->setSharedRenderer(renderer);
sensorsBargraph->setElementId("background");
sensorsBargraph->setObjectName("background");
m_ui->sensorsBargraph->scene()->addItem(sensorsBargraph);
m_ui->sensorsBargraph->setSceneRect(sensorsBargraph->boundingRect());
// Initialize the 9 bargraph values:
QMatrix lineMatrix = renderer->matrixForElement("accel_x");
QRectF rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_x"));
qreal startX = rect.x();
qreal startY = rect.y() + rect.height();
// maxBarHeight will be used for scaling it later.
maxBarHeight = rect.height();
// Then once we have the initial location, we can put it
// into a QGraphicsSvgItem which we will display at the same
// place: we do this so that the heading scale can be clipped to
// the compass dial region.
accel_x = new QGraphicsSvgItem();
accel_x->setSharedRenderer(renderer);
accel_x->setElementId("accel_x");
m_ui->sensorsBargraph->scene()->addItem(accel_x);
accel_x->setPos(startX, startY);
accel_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("accel_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_y"));
startX = rect.x();
startY = rect.y() + rect.height();
accel_y = new QGraphicsSvgItem();
accel_y->setSharedRenderer(renderer);
accel_y->setElementId("accel_y");
m_ui->sensorsBargraph->scene()->addItem(accel_y);
accel_y->setPos(startX, startY);
accel_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("accel_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_z"));
startX = rect.x();
startY = rect.y() + rect.height();
accel_z = new QGraphicsSvgItem();
accel_z->setSharedRenderer(renderer);
accel_z->setElementId("accel_z");
m_ui->sensorsBargraph->scene()->addItem(accel_z);
accel_z->setPos(startX, startY);
accel_z->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_x");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_x"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_x = new QGraphicsSvgItem();
gyro_x->setSharedRenderer(renderer);
gyro_x->setElementId("gyro_x");
m_ui->sensorsBargraph->scene()->addItem(gyro_x);
gyro_x->setPos(startX, startY);
gyro_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_y"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_y = new QGraphicsSvgItem();
gyro_y->setSharedRenderer(renderer);
gyro_y->setElementId("gyro_y");
m_ui->sensorsBargraph->scene()->addItem(gyro_y);
gyro_y->setPos(startX, startY);
gyro_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_z"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_z = new QGraphicsSvgItem();
gyro_z->setSharedRenderer(renderer);
gyro_z->setElementId("gyro_z");
m_ui->sensorsBargraph->scene()->addItem(gyro_z);
gyro_z->setPos(startX, startY);
gyro_z->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_x");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_x"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_x = new QGraphicsSvgItem();
mag_x->setSharedRenderer(renderer);
mag_x->setElementId("mag_x");
m_ui->sensorsBargraph->scene()->addItem(mag_x);
mag_x->setPos(startX, startY);
mag_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_y"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_y = new QGraphicsSvgItem();
mag_y->setSharedRenderer(renderer);
mag_y->setElementId("mag_y");
m_ui->sensorsBargraph->scene()->addItem(mag_y);
mag_y->setPos(startX, startY);
mag_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_z"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_z = new QGraphicsSvgItem();
mag_z->setSharedRenderer(renderer);
mag_z->setElementId("mag_z");
m_ui->sensorsBargraph->scene()->addItem(mag_z);
mag_z->setPos(startX, startY);
mag_z->setTransform(QTransform::fromScale(1, 0), true);
m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this));
m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true);
m_ui->calibrationVisualHelp->setRenderHint(QPainter::SmoothPixmapTransform, true);
displayVisualHelp("empty");
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
// will be dealing with some null pointers
addUAVObject("RevoCalibration");
addUAVObject("EKFConfiguration");
addUAVObject("HomeLocation");
addUAVObject("AttitudeSettings");
addUAVObject("RevoSettings");
@ -238,11 +108,46 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
// note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue.
m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
// six point calibrations
connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(accelStart()));
connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(magStart()));
connect(m_ui->sixPointsSave, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(savePositionData()));
connect(m_sixPointCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_sixPointCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_sixPointCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
connect(m_sixPointCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
connect(m_sixPointCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool)));
m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel(this);
// level calibration
connect(m_ui->boardLevelStart, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(start()));
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(savePosition()));
connect(m_levelCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_levelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->boardLevelSavePos, SLOT(setEnabled(bool)));
connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->boardLevelProgress, SLOT(setValue(int)));
// Connect the signals
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(doStartAccelGyroBiasCalibration()));
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibration()));
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ui->noiseMeasurementStart, SIGNAL(clicked()), this, SLOT(doStartNoiseMeasurement()));
// gyro zero calibration
m_gyroBiasCalibrationModel = new OpenPilot::GyroBiasCalibrationModel(this);
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), m_gyroBiasCalibrationModel, SLOT(start()));
connect(m_gyroBiasCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->gyroBiasProgress, SLOT(setValue(int)));
connect(m_gyroBiasCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_gyroBiasCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_gyroBiasCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
connect(m_gyroBiasCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
connect(m_gyroBiasCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_gyroBiasCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
@ -256,6 +161,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
populateWidgets();
refreshWidgetsValues();
m_ui->tabWidget->setCurrentIndex(0);
enableAllCalibrations();
}
ConfigRevoWidget::~ConfigRevoWidget()
@ -266,614 +172,17 @@ ConfigRevoWidget::~ConfigRevoWidget()
void ConfigRevoWidget::showEvent(QShowEvent *event)
{
Q_UNUSED(event)
// Thit fitInView method should only be called now, once the
// widget is shown, otherwise it cannot compute its values and
// the result is usually a sensorsBargraph that is way too small.
m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
Q_UNUSED(event);
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
m_thermalCalibrationModel->init();
}
void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
{
Q_UNUSED(event)
m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
Q_UNUSED(event);
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
}
/**
* Starts an accelerometer bias calibration.
*/
void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
{
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
m_ui->accelBiasStart->setEnabled(false);
m_ui->accelBiasProgress->setValue(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
UAVObject::Metadata mdata;
/* Need to get as many accel updates as possible */
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accelState->setMetadata(mdata);
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
}
/**
Updates the accel bias raw values
*/
void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case AccelState::OBJID:
{
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
break;
}
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)accel_accum_x.size() / (double)NOISE_SAMPLES;
double p2 = (double)accel_accum_y.size() / (double)NOISE_SAMPLES;
m_ui->accelBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100);
if (accel_accum_x.size() >= NOISE_SAMPLES &&
gyro_accum_y.size() >= NOISE_SAMPLES &&
collectingData == true) {
collectingData = false;
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
m_ui->accelBiasStart->setEnabled(true);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update the biases based on collected data
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += listMean(accel_accum_x);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += listMean(accel_accum_y);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += listMean(gyro_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += listMean(gyro_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += listMean(gyro_accum_z);
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);
// Recall saved board rotation
recallBoardRotation();
}
}
int LinearEquationsSolving(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
{
double fMaxElem;
double fAcc;
int i, j, k, m;
for (k = 0; k < (nDim - 1); k++) { // base row of matrix
// search of line with max element
fMaxElem = fabs(pfMatr[k * nDim + k]);
m = k;
for (i = k + 1; i < nDim; i++) {
if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
fMaxElem = pfMatr[i * nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if (m != k) {
for (i = k; i < nDim; i++) {
fAcc = pfMatr[k * nDim + i];
pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
pfMatr[m * nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if (pfMatr[k * nDim + k] == 0.) {
return 0; // needs improvement !!!
}
// triangulation of matrix with coefficients
for (j = (k + 1); j < nDim; j++) { // current row of matrix
fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
for (i = k; i < nDim; i++) {
pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
}
pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
}
}
for (k = (nDim - 1); k >= 0; k--) {
pfSolution[k] = pfVect[k];
for (i = (k + 1); i < nDim; i++) {
pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
}
return 1;
}
int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
{
int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i = 0; i < 5; i++) {
A[i][0] = 2.0 * (x[i + 1] - x[i]);
A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
A[i][2] = 2.0 * (y[i + 1] - y[i]);
A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
A[i][4] = 2.0 * (z[i + 1] - z[i]);
f[i] = x[i] * x[i] - x[i + 1] * x[i + 1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if (!LinearEquationsSolving(5, (double *)A, f, c)) {
return 0;
}
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
S[0] = Sx;
b[0] = Sx * c[0];
S[1] = sqrt(c[1] * Sx * Sx);
b[1] = c[2] * Sx * Sx / S[1];
S[2] = sqrt(c[3] * Sx * Sx);
b[2] = c[4] * Sx * Sx / S[2];
return 1;
}
/********** Functions for six point calibration **************/
/**
* Called by the "Start" button. Sets up the meta data and enables the
* buttons to perform six point calibration of the magnetometer (optionally
* accel) to compute the scale and bias of this sensor based on the current
* home location magnetic strength.
*/
void ConfigRevoWidget::doStartSixPointCalibration()
{
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
// check if Homelocation is set
if (!homeLocationData.Set) {
QMessageBox msgBox;
msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.setIcon(QMessageBox::Information);
msgBox.exec();
return;
}
#ifdef SIX_POINT_CAL_ACCEL
// Calibration accel
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
#endif
// Calibration mag
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 1;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
// Disable adaptive mag nulling
initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
revoCalibrationData.MagBiasNullingRate = 0;
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
Thread::usleep(100000);
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
UAVObject::Metadata mdata;
#ifdef SIX_POINT_CAL_ACCEL
/* Need to get as many accel updates as possible */
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accelState->setMetadata(mdata);
#endif
/* Need to get as many mag updates as possible */
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
initialMagStateMdata = mag->getMetadata();
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
/* Show instructions and enable controls */
m_ui->sixPointCalibInstructions->clear();
m_ui->sixPointCalibInstructions->append("Place horizontally and click save position...");
displayPlane("plane-horizontal");
m_ui->sixPointsStart->setEnabled(false);
m_ui->sixPointsSave->setEnabled(true);
position = 0;
}
/**
* Saves the data from the aircraft in one of six positions.
* This is called when they click "save position" and starts
* averaging data for this position.
*/
void ConfigRevoWidget::savePositionData()
{
QMutexLocker lock(&sensorsUpdateLock);
m_ui->sixPointsSave->setEnabled(false);
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
collectingData = true;
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
m_ui->sixPointCalibInstructions->append("Hold...");
}
/**
* Grab a sample of mag (optionally accel) data while in this position and
* store it for averaging. When sufficient points are collected advance
* to the next position (give message to user) or compute the scale and bias
*/
void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
// This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) {
if (obj->getObjID() == AccelState::OBJID) {
#ifdef SIX_POINT_CAL_ACCEL
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
#endif
} else if (obj->getObjID() == MagState::OBJID) {
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
MagState::DataFields magData = mag->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
} else {
Q_ASSERT(0);
}
}
#ifdef SIX_POINT_CAL_ACCEL
if (accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) {
#else
if (mag_accum_x.size() >= 20 && collectingData == true) {
#endif
collectingData = false;
m_ui->sixPointsSave->setEnabled(true);
#ifdef SIX_POINT_CAL_ACCEL
// Store the mean for this position for the accel
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
accel_data_x[position] = listMean(accel_accum_x);
accel_data_y[position] = listMean(accel_accum_y);
accel_data_z[position] = listMean(accel_accum_z);
#endif
// Store the mean for this position for the mag
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
mag_data_x[position] = listMean(mag_accum_x);
mag_data_y[position] = listMean(mag_accum_y);
mag_data_z[position] = listMean(mag_accum_z);
position = (position + 1) % 6;
if (position == 1) {
m_ui->sixPointCalibInstructions->append("Place with left side down and click save position...");
displayPlane("plane-left");
}
if (position == 2) {
m_ui->sixPointCalibInstructions->append("Place upside down and click save position...");
displayPlane("plane-flip");
}
if (position == 3) {
m_ui->sixPointCalibInstructions->append("Place with right side down and click save position...");
displayPlane("plane-right");
}
if (position == 4) {
m_ui->sixPointCalibInstructions->append("Place with nose up and click save position...");
displayPlane("plane-up");
}
if (position == 5) {
m_ui->sixPointCalibInstructions->append("Place with nose down and click save position...");
displayPlane("plane-down");
}
if (position == 0) {
computeScaleBias();
m_ui->sixPointsStart->setEnabled(true);
m_ui->sixPointsSave->setEnabled(false);
/* Cleanup original settings */
#ifdef SIX_POINT_CAL_ACCEL
accelState->setMetadata(initialAccelStateMdata);
#endif
mag->setMetadata(initialMagStateMdata);
// Recall saved board rotation
recallBoardRotation();
}
}
}
/**
* Computes the scale and bias for the magnetomer and (compile option)
* for the accel once all the data has been collected in 6 positions.
*/
void ConfigRevoWidget::computeScaleBias()
{
double S[3], b[3];
double Be_length;
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation);
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
#ifdef SIX_POINT_CAL_ACCEL
// Calibration accel
SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
#endif
// Calibration mag
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = -sign(S[0]) * b[0];
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = -sign(S[1]) * b[1];
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = -sign(S[2]) * b[2];
// Restore the previous setting
revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
#ifdef SIX_POINT_CAL_ACCEL
bool good_calibration = true;
// Check the mag calibration is good
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
// Check the accel calibration is good
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
if (good_calibration) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#else // ifdef SIX_POINT_CAL_ACCEL
bool good_calibration = true;
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
if (good_calibration) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#endif // ifdef SIX_POINT_CAL_ACCEL
position = -1; // set to run again
}
void ConfigRevoWidget::storeAndClearBoardRotation()
{
@ -912,232 +221,28 @@ void ConfigRevoWidget::recallBoardRotation()
}
/**
Rotate the paper plane
Show the selected visual aid
*/
void ConfigRevoWidget::displayPlane(QString elementID)
void ConfigRevoWidget::displayVisualHelp(QString elementID)
{
paperplane->setElementId(elementID);
m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
m_ui->calibrationVisualHelp->scene()->clear();
QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png");
m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap);
m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect());
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
}
/*********** Noise measurement functions **************/
/**
* Connect sensor updates and timeout for measuring the noise
*/
void ConfigRevoWidget::doStartNoiseMeasurement()
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
{
QMutexLocker lock(&sensorsUpdateLock);
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
Q_UNUSED(lock);
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
// check if Homelocation is set
if (!homeLocationData.Set) {
QMessageBox msgBox;
msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.setIcon(QMessageBox::Information);
msgBox.exec();
return;
if (replace || instructions.isNull()) {
m_ui->calibrationInstructions->clear();
}
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
/* Need to get as many accel, mag and gyro updates as possible */
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
UAVObject::Metadata mdata;
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accelState->setMetadata(mdata);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
initialMagStateMdata = mag->getMetadata();
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
/* Connect for updates */
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
}
/**
* Called when any of the sensors are updated. Stores the sample for measuring the
* variance at the end
*/
void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
Q_ASSERT(obj);
switch (obj->getObjID()) {
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroData = gyroState->getData();
gyro_accum_x.append(gyroData.x);
gyro_accum_y.append(gyroData.y);
gyro_accum_z.append(gyroData.z);
break;
}
case AccelState::OBJID:
{
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
break;
}
case MagState::OBJID:
{
MagState *mags = MagState::GetInstance(getObjectManager());
Q_ASSERT(mags);
MagState::DataFields magData = mags->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
break;
}
default:
Q_ASSERT(0);
}
float p1 = (float)mag_accum_x.length() / (float)NOISE_SAMPLES;
float p2 = (float)gyro_accum_x.length() / (float)NOISE_SAMPLES;
float p3 = (float)accel_accum_x.length() / (float)NOISE_SAMPLES;
float prog = (p1 < p2) ? p1 : p2;
prog = (prog < p3) ? prog : p3;
m_ui->noiseMeasurementProgress->setValue(prog * 100);
if (mag_accum_x.length() >= NOISE_SAMPLES &&
gyro_accum_x.length() >= NOISE_SAMPLES &&
accel_accum_x.length() >= NOISE_SAMPLES) {
// No need to for more updates
MagState *mags = MagState::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(mags, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(ekfConfiguration);
if (ekfConfiguration) {
EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData();
revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x);
revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y);
revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z);
revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x);
revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y);
revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z);
revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x);
revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y);
revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z);
ekfConfiguration->setData(revoCalData);
}
// Recall saved board rotation
recallBoardRotation();
if (!instructions.isNull()) {
m_ui->calibrationInstructions->append(instructions);
}
}
/********** UI Functions *************/
/**
Draws the sensor variances bargraph
*/
void ConfigRevoWidget::drawVariancesGraph()
{
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(ekfConfiguration);
if (!ekfConfiguration) {
return;
}
EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData();
// The expected range is from 1E-6 to 1E-1
double steps = 6; // 6 bars on the graph
float accel_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
if (accel_x) {
accel_x->setTransform(QTransform::fromScale(1, accel_x_var), false);
}
float accel_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
if (accel_y) {
accel_y->setTransform(QTransform::fromScale(1, accel_y_var), false);
}
float accel_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
if (accel_z) {
accel_z->setTransform(QTransform::fromScale(1, accel_z_var), false);
}
float gyro_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
if (gyro_x) {
gyro_x->setTransform(QTransform::fromScale(1, gyro_x_var), false);
}
float gyro_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
if (gyro_y) {
gyro_y->setTransform(QTransform::fromScale(1, gyro_y_var), false);
}
float gyro_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
if (gyro_z) {
gyro_z->setTransform(QTransform::fromScale(1, gyro_z_var), false);
}
// Scale by 1e-3 because mag vars are much higher.
float mag_x_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
if (mag_x) {
mag_x->setTransform(QTransform::fromScale(1, mag_x_var), false);
}
float mag_y_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
if (mag_y) {
mag_y->setTransform(QTransform::fromScale(1, mag_y_var), false);
}
float mag_z_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
if (mag_z) {
mag_z->setTransform(QTransform::fromScale(1, mag_z_var), false);
}
}
/**
* Called by the ConfigTaskWidget parent when RevoCalibration is updated
@ -1147,11 +252,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
{
ConfigTaskWidget::refreshWidgetsValues(object);
drawVariancesGraph();
m_ui->noiseMeasurementStart->setEnabled(true);
m_ui->sixPointsStart->setEnabled(true);
m_ui->accelBiasStart->setEnabled(true);
m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
m_ui->isSetCheckBox->setEnabled(false);
@ -1180,3 +281,21 @@ void ConfigRevoWidget::clearHomeLocation()
homeLocationData.Set = HomeLocation::SET_FALSE;
homeLocation->setData(homeLocationData);
}
void ConfigRevoWidget::disableAllCalibrations()
{
m_ui->sixPointsStartAccel->setEnabled(false);
m_ui->sixPointsStartMag->setEnabled(false);
m_ui->boardLevelStart->setEnabled(false);
m_ui->gyroBiasStart->setEnabled(false);
m_ui->ThermalBiasStart->setEnabled(false);
}
void ConfigRevoWidget::enableAllCalibrations()
{
m_ui->sixPointsStartAccel->setEnabled(true);
m_ui->sixPointsStartMag->setEnabled(true);
m_ui->boardLevelStart->setEnabled(true);
m_ui->gyroBiasStart->setEnabled(true);
m_ui->ThermalBiasStart->setEnabled(true);
}

80
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -29,7 +29,6 @@
#include "ui_revosensors.h"
#include "configtaskwidget.h"
#include "../uavobjectwidgetutils/configtaskwidget.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
@ -41,8 +40,12 @@
#include <QTimer>
#include <QMutex>
#include "calibration/thermal/thermalcalibrationmodel.h"
#include "calibration/sixpointcalibrationmodel.h"
#include "calibration/levelcalibrationmodel.h"
#include "calibration/gyrobiascalibrationmodel.h"
class Ui_Widget;
class ConfigRevoWidget : public ConfigTaskWidget {
Q_OBJECT
@ -51,83 +54,32 @@ public:
~ConfigRevoWidget();
private:
void drawVariancesGraph();
void displayPlane(QString elementID);
// ! Computes the scale and bias of the mag based on collected data
void computeScaleBias();
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
OpenPilot::LevelCalibrationModel *m_levelCalibrationModel;
OpenPilot::GyroBiasCalibrationModel *m_gyroBiasCalibrationModel;
Ui_RevoSensorsWidget *m_ui;
QGraphicsSvgItem *paperplane;
QGraphicsSvgItem *sensorsBargraph;
QGraphicsSvgItem *accel_x;
QGraphicsSvgItem *accel_y;
QGraphicsSvgItem *accel_z;
QGraphicsSvgItem *gyro_x;
QGraphicsSvgItem *gyro_y;
QGraphicsSvgItem *gyro_z;
QGraphicsSvgItem *mag_x;
QGraphicsSvgItem *mag_y;
QGraphicsSvgItem *mag_z;
QMutex sensorsUpdateLock;
double maxBarHeight;
int phaseCounter;
const static double maxVarValue;
const static int calibrationDelay = 10;
bool collectingData;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
QList<double> accel_accum_x;
QList<double> accel_accum_y;
QList<double> accel_accum_z;
QList<double> mag_accum_x;
QList<double> mag_accum_y;
QList<double> mag_accum_z;
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
double mag_data_x[6], mag_data_y[6], mag_data_z[6];
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialMagStateMdata;
UAVObject::Metadata initialBaroSensorMdata;
float initialMagCorrectionRate;
int position;
static const int NOISE_SAMPLES = 100;
// Board rotation store/recall
qint16 storedBoardRotation[3];
bool isBoardRotationStored;
void storeAndClearBoardRotation();
void recallBoardRotation();
private slots:
void displayVisualHelp(QString elementID);
void storeAndClearBoardRotation();
void recallBoardRotation();
void displayInstructions(QString instructions = QString(), bool replace = false);
// ! Overriden method from the configTaskWidget to update UI
virtual void refreshWidgetsValues(UAVObject *object = NULL);
// Slots for calibrating the mags
void doStartSixPointCalibration();
void doGetSixPointCalibrationMeasurement(UAVObject *obj);
void savePositionData();
// Slots for calibrating the accel and gyro
void doStartAccelGyroBiasCalibration();
void doGetAccelGyroBiasData(UAVObject *);
// Slots for measuring the sensor noise
void doStartNoiseMeasurement();
void doGetNoiseSample(UAVObject *);
// Slot for clearing home location
void clearHomeLocation();
void disableAllCalibrations();
void enableAllCalibrations();
protected:
void showEvent(QShowEvent *event);
void resizeEvent(QResizeEvent *event);

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-dwn.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 150 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-enu.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 86 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-ned.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-suw.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-swd.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 117 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-use.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 56 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-wds.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 176 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/empty.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-dwn.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 115 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-enu.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-horizontal-rotated.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-ned.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 70 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-suw.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 61 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-swd.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 79 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-use.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-wds.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 113 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/planes.xcf Normal file
View File

Binary file not shown.

309
ground/openpilotgcs/src/plugins/config/images/paper-plane.svg
View File

@ -1,309 +0,0 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="744.09448819"
height="1052.3622047"
id="svg2"
version="1.1"
inkscape:version="0.47 r22583"
sodipodi:docname="paper-plane.svg">
<title
id="title2859">Paper planes</title>
<defs
id="defs4">
<inkscape:path-effect
effect="envelope"
id="path-effect3160"
is_visible="true"
yy="true"
xx="true"
bendpath1="m 447.14285,362.36218 55.71429,0"
bendpath2="m 502.85714,362.36218 0,54.28572"
bendpath3="m 447.14285,416.6479 55.71429,0"
bendpath4="m 447.14285,362.36218 0,54.28572" />
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
<inkscape:perspective
id="perspective3143"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 618.71844 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="1486.9843 : 618.71844 : 1"
inkscape:persp3d-origin="743.49213 : 412.47896 : 1"
id="perspective2567" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.60408995"
inkscape:cx="160.9057"
inkscape:cy="659.88675"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
showguides="true"
inkscape:guide-bbox="true"
inkscape:window-width="1366"
inkscape:window-height="693"
inkscape:window-x="0"
inkscape:window-y="24"
inkscape:window-maximized="1" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title>Paper planes</dc:title>
<dc:creator>
<cc:Agent>
<dc:title>Edouard Lafargue</dc:title>
</cc:Agent>
</dc:creator>
<cc:license
rdf:resource="http://creativecommons.org/licenses/by-sa/3.0/" />
<dc:date>2010.08.29</dc:date>
</cc:Work>
<cc:License
rdf:about="http://creativecommons.org/licenses/by-sa/3.0/">
<cc:permits
rdf:resource="http://creativecommons.org/ns#Reproduction" />
<cc:permits
rdf:resource="http://creativecommons.org/ns#Distribution" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#Notice" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#Attribution" />
<cc:permits
rdf:resource="http://creativecommons.org/ns#DerivativeWorks" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#ShareAlike" />
</cc:License>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1">
<g
id="plane-flip"
inkscape:label="#g4365">
<g
transform="translate(305.77675,-285.13719)"
id="g3972">
<path
sodipodi:nodetypes="ccccccc"
id="path3974"
d="m 185.71429,456.6479 110,0 10,-30 8.57143,30 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
id="path3978"
d="m 314.66252,456.62472 68.1853,-118.18785"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
transform="translate(70,-226)"
id="path3982"
d="m 235.87062,652.34177 76.77159,-88.13581"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
</g>
<g
id="plane-horizontal"
inkscape:label="#g4349">
<g
inkscape:label="#g3946"
transform="translate(-158.56854,-296.98485)"
id="bla">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
id="path2822"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 296.42857,456.6479 383.21429,338.43361"
id="path2826" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 314.66252,456.62472 68.1853,-118.18785"
id="path2828" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 305.57115,485.16153 15.65736,-28.28427"
id="path2830" />
</g>
<path
id="path4065"
d="m 146.38783,182.85785 c -0.32605,-0.90165 -2.2117,-6.11664 -4.19034,-11.58888 -1.97865,-5.47224 -3.49043,-10.36632 -3.35952,-10.87573 0.28104,-1.0936 72.47583,-99.552799 72.77633,-99.252297 0.19834,0.198344 -52.99468,92.816837 -55.68619,96.959537 -0.73197,1.12664 -2.97014,7.44751 -4.97371,14.04639 -2.00356,6.59887 -3.7173,12.07724 -3.8083,12.17415 -0.091,0.0969 -0.43223,-0.56152 -0.75827,-1.46317 l 0,0 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-left"
inkscape:label="#g4357">
<g
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,780.50589,449.7941)"
id="g3994">
<path
sodipodi:nodetypes="ccccccc"
id="path3996"
d="m 185.71429,456.6479 110,0 10.14561,-28.97991 8.42582,28.97991 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
id="path4000"
d="m 314.66252,456.62472 68.1853,-118.18785"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 295.91153,456.54311 308.5568,440.11449"
id="path4006" />
<path
sodipodi:nodetypes="cc"
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,675.67433,927.38588)"
id="path4008"
d="m 498.79912,370.56922 89.10071,-76.12668"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
<path
id="path4075"
d="m 331.59817,148.54231 c 0.90991,-0.75045 3.69622,-2.88986 6.1918,-4.75425 l 4.53742,-3.3898 3.80262,1.08903 c 2.09145,0.59896 3.80263,1.2469 3.80263,1.43986 0,0.30789 -18.78631,6.9796 -19.65325,6.9796 -0.18458,0 0.40887,-0.614 1.31878,-1.36444 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-right"
inkscape:label="#g4371">
<g
id="g3984"
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,1070.5332,666.10797)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
id="path3986"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 296.42857,456.6479 383.21429,338.43361"
id="path3988" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 314.66252,456.62472 68.1853,-118.18785"
id="path3990" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 305.57115,485.16153 15.65736,-28.28427"
id="path3992" />
</g>
<path
id="path4077"
d="m 602.06162,363.33367 c -6.43591,-2.39032 -11.37138,-4.37774 -10.96771,-4.41647 0.40366,-0.0387 5.79069,-1.627 11.97117,-3.52947 9.393,-2.89135 18.46611,-7.60947 55.28388,-28.74823 24.22566,-13.90905 44.2519,-25.08393 44.50276,-24.83307 0.25087,0.25086 -19.69141,15.17498 -44.31616,33.1647 l -44.77228,32.70858 -11.70166,-4.34604 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-up"
inkscape:label="#g4391">
<g
transform="translate(-158.56854,5.01515)"
id="g4026">
<path
sodipodi:nodetypes="ccccccc"
id="path4012"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
id="path4014"
d="m 296.42857,456.6479 9.46902,-235.74646"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
id="path4016"
d="M 314.66252,456.62472 305.89759,218.42163"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
transform="translate(158.56854,-5.01515)"
id="path4022"
d="m 147.32905,489.5321 0,-4.13846 0,-263.20584"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
id="path4024"
sodipodi:nodetypes="ccccccc" />
</g>
<path
id="path4081"
d="m 142.14371,473.11345 -3.82369,-10.68763 0.40417,-5.40719 c 0.22229,-2.97395 1.94295,-44.12254 3.82369,-91.4413 l 3.41951,-86.03413 0.14953,51.06447 c 0.0823,28.08546 0.0823,74.04349 0,102.12894 l -0.14953,51.06447 -3.82368,-10.68763 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
<path
id="path4083"
d="m 148.16566,377.28265 c 0.0301,-58.26325 0.13214,-104.87969 0.22683,-103.59211 0.0947,1.28759 1.67362,43.76715 3.50873,94.39902 l 3.33657,92.05795 -2.16196,7.14466 c -1.18907,3.92956 -2.7926,9.11993 -3.56339,11.53415 l -1.40144,4.3895 0.0547,-105.93317 0,0 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-down"
inkscape:label="#g4401">
<g
id="g4033"
transform="matrix(1,0,0,-1,-158.56854,1000.7047)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
id="path4035"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 296.42857,456.6479 9.46902,-235.74646"
id="path4037"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 314.66252,456.62472 305.89759,218.42163"
id="path4039"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 147.32905,489.5321 0,-4.13846 0,-263.20584"
id="path4041"
transform="translate(158.56854,-5.01515)" />
<path
sodipodi:nodetypes="ccccccc"
id="path4043"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
<path
id="path4085"
d="m 148.16566,628.36182 -0.0547,-105.93317 1.40144,4.3895 c 0.77079,2.41422 2.37432,7.60459 3.56339,11.53415 l 2.16196,7.14466 -3.33657,92.05795 c -1.83511,50.63187 -3.41404,93.11143 -3.50873,94.39902 -0.0947,1.28758 -0.19676,-45.32887 -0.22683,-103.59211 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
<path
id="path4087"
d="m 142.54602,640.35978 c -1.88175,-47.47972 -3.6008,-88.73996 -3.8201,-91.68942 l -0.39873,-5.36266 3.8201,-10.73216 3.8201,-10.73216 0.14953,51.01814 c 0.0822,28.05998 0.0822,74.14969 0,102.42158 l -0.14953,51.40343 -3.42137,-86.32675 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
</g>
</svg>
Side by Side

Before

Width:  |  Height:  |  Size: 14 KiB

376
ground/openpilotgcs/src/plugins/config/revosensors.ui
View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>836</width>
<height>527</height>
<height>605</height>
</rect>
</property>
<property name="windowTitle">
@ -62,118 +62,224 @@
<property name="verticalSpacing">
<number>6</number>
</property>
<item row="0" column="0" rowspan="4">
<layout class="QVBoxLayout">
<item>
<widget class="QGraphicsView" name="calibrationVisualHelp">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</item>
<item row="4" column="0" colspan="2">
<layout class="QVBoxLayout" name="instruction_layout">
<item>
<widget class="QTextEdit" name="calibrationInstructions">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Calibration status</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="sizeAdjustPolicy">
<enum>QAbstractScrollArea::AdjustToContents</enum>
</property>
</widget>
</item>
<item>
<widget class="QTextEdit" name="textEdit">
<property name="toolTip">
<string>Calibration instructions</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="html">
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Ubuntu'; font-size:9pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
&lt;tr&gt;
&lt;td style=&quot;border: none;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:16pt; font-weight:600; font-style:italic;&quot;&gt;Help&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps you achieve the best possible results.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:8pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#1: Multi-Point calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This calibration will compute the scale for Magnetometer or Accelerometer sensors. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Press &amp;quot;Calibrate Mag&amp;quot; or &amp;quot;Calibrate Accel&amp;quot; to begin calibration, and follow the instructions which will be displayed here. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;For optimal calibration perform the Accel calibration with the board not mounted to the craft. in this way you can accurately level the board on your desk/table during the process. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt; font-weight:600; font-style:italic;&quot;&gt;Note 1:&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt; Your &lt;/span&gt;&lt;span style=&quot; font-size:11pt; font-weight:600;&quot;&gt;HomeLocation must be set first&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt; font-weight:600; font-style:italic;&quot;&gt;Note 2:&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt; There is no need to point exactly at South/North/West/East. They are just used to easily tells the user how to point the plane/craft. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#2: Board level calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#3: Gyro Bias calculation&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This step allows to calibrate the gyro measured value when the board is steady. To perform the calibration leave the board/aircraft completely steady and click start. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#4: Thermal Calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;The calibration will compute sensors bias variations at different temperatures while the board warms up.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed&lt;/span&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#1: Accelerometer/Magnetometer calibration</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QPushButton" name="sixPointsStartAccel">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Launch accelerometer range and bias calibration.</string>
</property>
<property name="text">
<string>Calibrate Accel</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="sixPointsStartMag">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Launch magnetometer range and bias calibration.</string>
</property>
<property name="text">
<string>Calibrate Mag</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_thermalbias_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="sixPointsSave">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Save settings (only enabled when calibration is running)</string>
</property>
<property name="text">
<string>Save Position</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#3: Sensor noise calibration</string>
<string>#2: Board level calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<layout class="QHBoxLayout">
<item>
<widget class="QGraphicsView" name="sensorsBargraph">
<property name="toolTip">
<string>These are the sensor variance values computed by the AHRS.
Tip: lower is better!</string>
<widget class="QPushButton" name="boardLevelStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="noiseMeasurementStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Press to start a calibration procedure, about 15 seconds.
Hint: run this with engines at cruising speed.</string>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="noiseMeasurementProgress">
<property name="enabled">
<bool>true</bool>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox_5">
<property name="title">
<string>#2: Magnetometer calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_sixPointCalib">
<item>
<widget class="QGraphicsView" name="sixPointsHelp">
<property name="toolTip">
<string>Nice paper plane, eh?</string>
<widget class="QProgressBar" name="boardLevelProgress">
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QPushButton" name="sixPointsStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Launch a sensor range and bias calibration.</string>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="sixPointsSave">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Save settings (only enabled when calibration is running)</string>
</property>
<property name="text">
<string>Save Position</string>
</property>
</widget>
</item>
</layout>
<widget class="QPushButton" name="boardLevelSavePos">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Save Position</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="0" column="0" colspan="2">
<item row="2" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#3: Gyro bias calibration</string>
</property>
<layout class="QHBoxLayout">
<item>
<widget class="QPushButton" name="gyroBiasStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="gyroBiasProgress">
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="3" column="1">
<widget class="QGroupBox" name="groupBox_5">
<property name="title">
<string>#1: Thermal calibration</string>
<string>#4*: Thermal calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_thermalbias">
<layout class="QVBoxLayout">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_thermalbias">
<layout class="QHBoxLayout">
<item>
<widget class="QLabel" name="label_temperature">
<property name="font">
@ -287,7 +393,7 @@
<number>0</number>
</property>
<property name="textVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>
@ -316,74 +422,6 @@
</layout>
</widget>
</item>
<item row="2" column="0" colspan="2">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#4: Accelerometer Bias calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QPushButton" name="accelBiasStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="accelBiasProgress">
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item row="3" column="0" colspan="2">
<widget class="QTextEdit" name="sixPointCalibInstructions">
<property name="toolTip">
<string>Six Point Calibration instructions</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="html">
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
&lt;tr&gt;
&lt;td style=&quot;border: none;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt; font-weight:600;&quot;&gt;Help&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;Step #1 and #2 and 3 are really necessary. Step #4 will help you achieve the best possible results.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#1: Thermal Calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;The calibration will compute the variation of all sensors bias at different temperatures while the board warm up.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed&lt;/span&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#1: Multi-Point calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This calibration will compute the scale for all sensors on the INS. Press &amp;quot;Start&amp;quot; to begin calibration, and follow the instructions which will be displayed here. Note that your HomeLocation must be set first, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#2: Sensor noise calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This calibration will compute sensor variance under standard conditions. You can leave your engines running at low to mid throttle to take their vibration into account before pressing &amp;quot;Start&amp;quot;.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#3: Accelerometer bias calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This step will ensure that accelerometer bias is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.&lt;/span&gt;&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@ -430,12 +468,12 @@ p, li { white-space: pre-wrap; }
</spacer>
</item>
<item row="1" column="3">
<widget class="QSpinBox" name="pitchRotation">
<widget class="QDoubleSpinBox" name="pitchRotation">
<property name="minimum">
<number>-90</number>
<double>-90.000000000000000</double>
</property>
<property name="maximum">
<number>90</number>
<double>90.000000000000000</double>
</property>
</widget>
</item>
@ -512,12 +550,12 @@ font:bold;</string>
</spacer>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="rollRotation">
<widget class="QDoubleSpinBox" name="rollRotation">
<property name="minimum">
<number>-180</number>
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<number>180</number>
<double>180.000000000000000</double>
</property>
</widget>
</item>
@ -545,12 +583,12 @@ font:bold;</string>
</widget>
</item>
<item row="1" column="5">
<widget class="QSpinBox" name="yawRotation">
<widget class="QDoubleSpinBox" name="yawRotation">
<property name="minimum">
<number>-180</number>
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<number>180</number>
<double>180.000000000000000</double>
</property>
</widget>
</item>

28
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp
View File

@ -228,34 +228,6 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
return objMngr;
}
double ConfigTaskWidget::listMean(QList<double> list)
{
double accum = 0;
for (int i = 0; i < list.size(); i++) {
accum += list[i];
}
return accum / list.size();
}
double ConfigTaskWidget::listVar(QList<double> list)
{
double mean_accum = 0;
double var_accum = 0;
double mean;
for (int i = 0; i < list.size(); i++) {
mean_accum += list[i];
}
mean = mean_accum / list.size();
for (int i = 0; i < list.size(); i++) {
var_accum += (list[i] - mean) * (list[i] - mean);
}
// Use unbiased estimator
return var_accum / (list.size() - 1);
}
void ConfigTaskWidget::onAutopilotDisconnect()
{

2
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h
View File

@ -106,8 +106,6 @@ public:
void saveObjectToSD(UAVObject *obj);
UAVObjectManager *getObjectManager();
static double listMean(QList<double> list);
static double listVar(QList<double> list);
void addUAVObject(QString objectName, QList<int> *reloadGroups = NULL);
void addUAVObject(UAVObject *objectName, QList<int> *reloadGroups = NULL);

2
make/apps-defs.mk
View File

@ -99,7 +99,7 @@ SRC += $(PIOSCOMMON)/pios_usb_util.c
## PIOS system code
SRC += $(PIOSCOMMON)/pios_task_monitor.c
SRC += $(PIOSCOMMON)/pios_callbackscheduler.c
SRC += $(PIOSCOMMON)/pios_notify.c
## Misc library functions
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/sanitycheck.c

4
make/common-defs.mk
View File

@ -141,6 +141,10 @@ ifdef EE_BANK_BASE
BOARD_CDEFS += -DEE_BANK_BASE=$(EE_BANK_BASE)
BOARD_CDEFS += -DEE_BANK_SIZE=$(EE_BANK_SIZE)
endif
ifdef USER_EE_BANK_BASE
BOARD_CDEFS += -DUSER_EE_BANK_BASE=$(USER_EE_BANK_BASE)
BOARD_CDEFS += -DUSER_EE_BANK_SIZE=$(USER_EE_BANK_SIZE)
endif
CDEFS += $(BOARD_CDEFS)
ifeq ($(DEBUG), YES)

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="AttitudeSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="BoardRotation" units="deg" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>

3
shared/uavobjectdefinition/revocalibration.xml
View File

@ -2,7 +2,8 @@
<object name="RevoCalibration" singleinstance="true" settings="true" category="Sensors">
<description>Settings for the INS to control the algorithm and what is updated</description>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
defaultvalue="1,0,0,0,1,0,0,0,1"/>
<!-- These settings are related to how the sensors are post processed -->
<!-- TODO: reimplement, put elsewhere (later) -->
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>

4
shared/uavobjectdefinition/revosettings.xml
View File

@ -7,6 +7,10 @@
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"/>
<!-- Configuration for magnetometer vector validity check -->
<field name="MagnetometerMaxDeviation" units="%" type="float" elementnames="Warning,Error" defaultvalue="0.05,0.15" />
<!-- Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature
bias = a + b * temp + c * temp^2 + d * temp^3 -->
<field name="BaroTempCorrectionPolynomial" units="" type="float" elements="4" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>

1
shared/uavobjectdefinition/systemalarms.xml
View File

@ -15,6 +15,7 @@
<elementname>Actuator</elementname>
<elementname>Attitude</elementname>
<elementname>Sensors</elementname>
<elementname>Magnetometer</elementname>
<elementname>Airspeed</elementname>
<elementname>Stabilization</elementname>
<elementname>Guidance</elementname>