rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' into corvuscorax/OP-947_stateestimator-module

Browse Source
This commit is contained in:
Corvus Corax 2013-06-12 18:32:01 +02:00
commit 1d17d08cca
16 changed files with 137 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
WHATSNEW.txt
View File

@ -170,14 +170,15 @@ list above. Some of them can be found using this link:
http://progress.openpilot.org/issues/?filter=10860
OP-678, OP-693, OP-719, OP-726, OP-727, OP-747, OP-761, OP-769, OP-770,
OP-772, OP-792, OP-804, OP-807, OP-812, OP-816, OP-817, OP-820, OP-821,
OP-843, OP-846, OP-854, OP-855, OP-856, OP-861, OP-864, OP-867, OP-871,
OP-873, OP-874, OP-875, OP-879, OP-885, OP-886, OP-888, OP-889, OP-890,
OP-891, OP-892, OP-893, OP-894, OP-895, OP-896, OP-897, OP-898, OP-899,
OP-900, OP-903, OP-905, OP-906, OP-907, OP-910, OP-912, OP-917, OP-920,
OP-925, OP-926, OP-928, OP-935, OP-936, OP-939, OP-952, OP-955, OP-957,
OP-968, OP-969, OP-970, OP-977, OP-980, OP-981, OP-982, OP-983, OP-987,
OP-988, OP-989
OP-772, OP-784, OP-792, OP-804, OP-807, OP-812, OP-816, OP-817, OP-820,
OP-821, OP-843, OP-846, OP-854, OP-855, OP-856, OP-861, OP-864, OP-867,
OP-871, OP-873, OP-874, OP-875, OP-879, OP-885, OP-886, OP-888, OP-889,
OP-890, OP-891, OP-892, OP-893, OP-894, OP-895, OP-896, OP-897, OP-898,
OP-899, OP-900, OP-903, OP-905, OP-906, OP-907, OP-910, OP-912, OP-917,
OP-920, OP-925, OP-926, OP-928, OP-935, OP-936, OP-939, OP-952, OP-955,
OP-957, OP-958, OP-965, OP-968, OP-969, OP-970, OP-976, OP-977, OP-980,
OP-981, OP-982, OP-983, OP-987, OP-988, OP-989, OP-990, OP-991, OP-993
OP-997, OP-998, OP-999
Short summary of changes. For a complete list see the git log.

33
flight/modules/AltitudeHold/altitudehold.c
View File

@ -63,6 +63,7 @@
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000
// Private types
// Private variables
@ -130,7 +131,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Force update of the settings
SettingsUpdatedCb(&ev);
// Failsafe handling
uint32_t lastAltitudeHoldDesiredUpdate = 0;
bool enterFailSafe = false;
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
BaroSensorConnectQueue(queue);
@ -144,6 +147,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Main task loop
bool baro_updated = false;
while (1) {
enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) {
@ -173,6 +177,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
starting_altitude = altHold.Altitude;
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
}
} else if (ev.obj == AccelStateHandle()) {
static uint32_t timeval;
@ -352,6 +357,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
}
if (!running) {
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
continue;
}
@ -370,24 +376,31 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
// reset the failsafe timer
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
AltitudeHoldDesiredGet(&altitudeHoldDesired);
}
}

7
flight/modules/ManualControl/manualcontrol.c
View File

@ -806,10 +806,15 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
*/
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{
const float DEADBAND = 0.10f;
const float DEADBAND = 0.25f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle
uint8_t throttleRate;
uint8_t throttleExp;

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

@ -53,6 +53,7 @@
#include <watchdogstatus.h>
#include <taskinfo.h>
#include <hwsettings.h>
#include <pios_flashfs.h>
// Flight Libraries
#include <sanitycheck.h>
@ -94,7 +95,7 @@ static xQueueHandle objectPersistenceQueue;
static bool stackOverflow;
static bool mallocFailed;
static HwSettingsData bootHwSettings;
static struct PIOS_FLASHFS_Stats fsStats;
// Private functions
static void objectUpdatedCb(UAVObjEvent *ev);
static void hwSettingsUpdatedCb(UAVObjEvent *ev);
@ -108,6 +109,10 @@ static void systemTask(void *parameters);
static void updateI2Cstats();
static void updateWDGstats();
#endif
extern uintptr_t pios_uavo_settings_fs_id;
extern uintptr_t pios_user_fs_id;
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
@ -462,7 +467,18 @@ static void updateStats()
if (idleCounterClear) {
idleCounter = 0;
}
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
if(pios_uavo_settings_fs_id){
PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
stats.SysSlotsFree = fsStats.num_free_slots;
stats.SysSlotsActive = fsStats.num_active_slots;
}
if(pios_user_fs_id){
PIOS_FLASHFS_GetStats(pios_user_fs_id, &fsStats);
stats.UsrSlotsFree = fsStats.num_free_slots;
stats.UsrSlotsActive = fsStats.num_active_slots;
}
#endif
portTickType now = xTaskGetTickCount();
if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms

16
flight/pios/common/pios_flashfs_logfs.c
View File

@ -1158,7 +1158,23 @@ out_end_trans:
out_exit:
return rc;
}
/**
* @brief Returs stats for the filesystems
* @param[in] fs_id The filesystem to use for this action
* @return 0 if success or error code
* @retval -1 if fs_id is not a valid filesystem instance
*/
int32_t PIOS_FLASHFS_GetStats(uintptr_t fs_id, struct PIOS_FLASHFS_Stats *stats){
PIOS_Assert(stats);
struct logfs_state *logfs = (struct logfs_state *)fs_id;
if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
return -1;
}
stats->num_active_slots = logfs->num_active_slots;
stats->num_free_slots = logfs->num_free_slots;
return 0;
}
#endif /* PIOS_INCLUDE_FLASH */
/**

7
flight/pios/inc/pios_flashfs.h
View File

@ -29,9 +29,14 @@
#include <stdint.h>
struct PIOS_FLASHFS_Stats{
uint16_t num_free_slots; /* slots in free state */
uint16_t num_active_slots; /* slots in active state */
};
int32_t PIOS_FLASHFS_Format(uintptr_t fs_id);
int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size);
int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size);
int32_t PIOS_FLASHFS_ObjDelete(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id);
int32_t PIOS_FLASHFS_GetStats(uintptr_t fs_id, struct PIOS_FLASHFS_Stats *stats);
#endif /* PIOS_FLASHFS_H */

7
flight/pios/stm32f4xx/pios_usb_hid.c
View File

@ -252,12 +252,6 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail)
return;
}
// add a timeout to prevent connection drops
static uint32_t last_rx_time_raw = 0;
if (PIOS_DELAY_DiffuS(last_rx_time_raw) > 1000000) {
usb_hid_dev->rx_active = false;
}
// If endpoint was stalled and there is now space make it valid
#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1;
@ -266,7 +260,6 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail)
#endif
if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) {
last_rx_time_raw = PIOS_DELAY_GetRaw();
PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep,
usb_hid_dev->rx_packet_buffer,
sizeof(usb_hid_dev->rx_packet_buffer));

12
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -77,7 +77,7 @@ uint32_t pios_com_hkosd_id;
uint32_t pios_usb_rctx_id;
uintptr_t pios_uavo_settings_fs_id;
uintptr_t pios_user_fs_id = 0;
/**
* Configuration for MPU6000 chip
*/
@ -192,6 +192,11 @@ void PIOS_Board_Init(void)
PIOS_DEBUG_Assert(0);
}
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
@ -232,11 +237,6 @@ void PIOS_Board_Init(void)
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
}
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

11
flight/targets/boards/oplinkmini/firmware/pios_board.c
View File

@ -91,6 +91,11 @@ void PIOS_Board_Init(void)
PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id);
#endif
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
@ -138,12 +143,6 @@ void PIOS_Board_Init(void)
}
OPLinkSettingsGet(&oplinkSettings);
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

10
flight/targets/boards/osd/firmware/pios_board.c
View File

@ -105,6 +105,7 @@ uint32_t pios_com_telem_usb_id;
uint32_t pios_com_telem_rf_id;
uintptr_t pios_uavo_settings_fs_id;
uintptr_t pios_user_fs_id = 0;
/**
* TIM3 is triggered by the HSYNC signal into its ETR line and will divide the
@ -171,6 +172,10 @@ void PIOS_Board_Init(void)
#error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS)
#endif
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
@ -186,11 +191,6 @@ void PIOS_Board_Init(void)
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

17
flight/targets/boards/revolution/board_hw_defs.c
View File

@ -677,9 +677,20 @@ const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revi
#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) */
static const struct flashfs_logfs_cfg flashfs_external_user_cfg = {
.fs_magic = 0x99abcdef,
.total_fs_size = 0x001C0000, /* 2M bytes (32 sectors = entire chip) */
.arena_size = 0x00010000, /* 256 * slot size */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = 0x40000, /* start at the beginning of the chip */
.sector_size = 0x00010000, /* 64K bytes */
.page_size = 0x00000100, /* 256 bytes */
};
static const struct flashfs_logfs_cfg flashfs_external_system_cfg = {
.fs_magic = 0x99bbcdef,
.total_fs_size = 0x00040000, /* 2M bytes (32 sectors = entire chip) */
.arena_size = 0x00010000, /* 256 * slot size */
.slot_size = 0x00000100, /* 256 bytes */

26
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -350,21 +350,16 @@ void PIOS_Board_Init(void)
/* 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_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);
}
// Initialize the external USER flash
if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_cfg, &pios_jedec_flash_driver, flash_id)) {
if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_external_system_cfg, &pios_jedec_flash_driver, flash_id)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_user_cfg, &pios_jedec_flash_driver, flash_id)) {
PIOS_DEBUG_Assert(0);
}
@ -387,6 +382,12 @@ void PIOS_Board_Init(void)
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_Init();
#endif
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
@ -395,11 +396,6 @@ void PIOS_Board_Init(void)
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

10
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -438,6 +438,11 @@ void PIOS_Board_Init(void)
PIOS_IAP_WriteBootCmd(2, 0);
}
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
@ -447,11 +452,6 @@ void PIOS_Board_Init(void)
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

10
flight/targets/boards/simposix/firmware/pios_board.c
View File

@ -118,6 +118,11 @@ void PIOS_Board_Init(void)
/* Delay system */
PIOS_DELAY_Init();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
@ -129,11 +134,6 @@ void PIOS_Board_Init(void)
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor */
if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
PIOS_Assert(0);
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();

6
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="StabilizationSettings" singleinstance="true" settings="true">
<description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description>
<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>

6
shared/uavobjectdefinition/systemstats.xml
View File

@ -9,9 +9,13 @@
<field name="EventSystemWarningID" units="uavoid" type="uint32" elements="1"/>
<field name="ObjectManagerCallbackID" units="uavoid" type="uint32" elements="1"/>
<field name="ObjectManagerQueueID" units="uavoid" type="uint32" elements="1"/>
<field name="SysSlotsFree" units="slots" type="uint16" elements="1"/>
<field name="SysSlotsActive" units="slots" type="uint16" elements="1"/>
<field name="UsrSlotsFree" units="slots" type="uint16" elements="1"/>
<field name="UsrSlotsActive" units="slots" type="uint16" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>
</xml>