void plan_setup_AutoTakeoff()
{
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
// We only allow takeoff if the state transition of disarmed to armed occurs
// whilst in the autotake flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
StabilizationDesiredData stabiDesired;
StabilizationDesiredGet(&stabiDesired);
// Are we inflight?
if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
// ok assume already in flight and just enter position hold
// if we are not actually inflight this will just be a violent autotakeoff
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
plan_setup_positionHold();
} else {
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
// Note that if this mode was invoked unintentionally whilst in flight, effectively
// all inputs get ignored and the vtol continues to fly to its previous
// stabi command.
}
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
/**
* If the system is disarmed, look for a variety of conditions that
* make it unsafe to arm (that might not be dangerous to engage once
* flying).
*/
static int32_t check_safe_to_arm()
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only arm in traditional modes where pilot has control
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_ACRO:
case FLIGHTSTATUS_FLIGHTMODE_ACROPLUS:
case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
break;
default:
// Any mode not specifically allowed prevents arming
return SYSTEMALARMS_CONFIGERROR_UNSAFETOARM;
}
}
return SYSTEMALARMS_CONFIGERROR_NONE;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
QuadMotorsDesiredData actuatorDesired;
FlightStatusData flightStatus;
//SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus);
QuadMotorsDesiredGet(&actuatorDesired);
//if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED)
//{
// ZERO MOTORS
//}
//else
//{
//}
//ActuatorSettingsData settings;
//ActuatorSettingsGet(&settings);
//PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);
PIOS_SetMKSpeed(0,actuatorDesired.motorFront_NW);
PIOS_SetMKSpeed(1,actuatorDesired.motorRight_NE);
PIOS_SetMKSpeed(2,actuatorDesired.motorBack_SE);
PIOS_SetMKSpeed(3,actuatorDesired.motorLeft_SW);
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
//! When the control system requests to disarm the FC
static int32_t control_event_disarm()
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
FlightStatusSet(&flightStatus);
}
return 0;
}
//! When the control system requests to start arming the FC
static int32_t control_event_arming()
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMING) {
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMING;
FlightStatusSet(&flightStatus);
}
return 0;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
portTickType lastSysTime;
/* create all modules thread */
MODULE_TASKCREATE_ALL
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectCallback(&objectUpdatedCb);
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
#if defined(DIAGNOSTICS)
updateI2Cstats();
updateWDGstats();
#endif
// Update the task status object
TaskMonitorUpdateAll();
// Flash the heartbeat LED
PIOS_LED_Toggle(LED1);
// Turn on the error LED if an alarm is set
#if (PIOS_LED_NUM > 1)
if (AlarmsHasErrors()) {
PIOS_LED_Toggle(LED2);
} else if (AlarmsHasWarnings()) {
PIOS_LED_On(LED2);
} else {
PIOS_LED_Off(LED2);
}
#endif
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Wait until next period
if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) );
} else {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
}
/**
* On changed waypoints or active waypoint update position desired
* if we are in charge
*/
static void waypointsUpdated(UAVObjEvent * ev)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
return;
WaypointActiveGet(&waypointActive);
if(active_waypoint != waypointActive.Index)
activateWaypoint(waypointActive.Index);
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* Select and use transmitter control
* @param [in] reset_controller True if previously another controller was used
*/
int32_t transmitter_control_select(bool reset_controller)
{
// Activate the flight mode corresponding to the switch position
set_flight_mode();
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
update_actuator_desired(&cmd);
break;
case FLIGHTSTATUS_FLIGHTMODE_ACRO:
case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
update_stabilization_desired(&cmd, &settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
altitude_hold_desired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
set_loiter_command(&cmd);
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
// The path planner module processes data here
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
break;
default:
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_UNDEFINED);
break;
}
lastFlightMode = flightStatus.FlightMode;
return 0;
}
/* library functions */
void FlightStatus_Get(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if (Param[0]->Val->Pointer == NULL)
return;
FlightStatusData data;
FlightStatusGet(&data);
// use the same struct like picoc. see below
struct mystruct {
unsigned char Armed;
unsigned char FlightMode;
unsigned char ControlSource;
} *pdata;
pdata = Param[0]->Val->Pointer;
pdata->Armed = data.Armed;
pdata->FlightMode = data.FlightMode;
pdata->ControlSource = data.ControlSource;
}
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (PIOS_heap_malloc_failed_p()) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
#if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
if (StabilizationSettingsHandle())
StabilizationSettingsConnectCallback(configurationUpdatedCb);
if (SystemSettingsHandle())
SystemSettingsConnectCallback(configurationUpdatedCb);
if (ManualControlSettingsHandle())
ManualControlSettingsConnectCallback(configurationUpdatedCb);
if (FlightStatusHandle())
FlightStatusConnectCallback(configurationUpdatedCb);
#endif
#if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
if (StateEstimationHandle())
StateEstimationConnectCallback(configurationUpdatedCb);
#endif
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
#if defined(WDG_STATS_DIAGNOSTICS)
updateWDGstats();
#endif
#if defined(DIAG_TASKS)
// Update the task status object
TaskMonitorUpdateAll();
#endif
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(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 (indicateError()) {
#if defined (PIOS_LED_ALARM)
PIOS_LED_On(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
} else {
#if defined (PIOS_LED_ALARM)
PIOS_LED_Off(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS;
if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
}
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
#if defined(I2C_WDG_STATS_DIAGNOSTICS)
updateI2Cstats();
updateWDGstats();
#endif
#if defined(DIAG_TASKS)
// Update the task status object
TaskMonitorUpdateAll();
#endif
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
#if defined (PIOS_LED_ALARM)
if (AlarmsHasWarnings()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
#endif /* PIOS_LED_ALARM */
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch;
else
rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw};
local_error[2] = fmod(local_error[2] + 180, 360) - 180;
#endif
for(uint8_t i = 0; i < MAX_AXES; i++) {
gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);
}
float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate
for(uint8_t i=0; i< MAX_AXES; i++)
{
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float weak_leveling = local_error[i] * weak_leveling_kp;
if(weak_leveling > weak_leveling_max)
weak_leveling = weak_leveling_max;
if(weak_leveling < -weak_leveling_max)
weak_leveling = -weak_leveling_max;
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);
}
break;
}
}
uint8_t shouldUpdate = 1;
RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
rateDesiredAxis[ct] = settings.MaximumRate[ct];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[ct] = -settings.MaximumRate[ct];
switch(stabDesired.StabilizationMode[ct])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]);
actuatorDesiredAxis[ct] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
switch (ct)
{
case ROLL:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
case PITCH:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
case YAW:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
}
break;
}
}
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
if(dT > 15)
actuatorDesired.NumLongUpdates++;
ActuatorDesiredSet(&actuatorDesired);
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod));
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
for (uint32_t i = 0; i < VTOL_PID_NUM; i++)
pid_zero(&vtol_pids[i]);
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
{
//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
enum AUTOTUNE_STATE state = AT_INIT;
portTickType lastUpdateTime = xTaskGetTickCount();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
// TODO:
// 1. get from queue
// 2. based on whether it is flightstatus or manualcontrol
portTickType diffTime;
const uint32_t PREPARE_TIME = 2000;
const uint32_t MEAURE_TIME = 30000;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only allow this module to run when autotuning
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
state = AT_INIT;
vTaskDelay(50);
continue;
}
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
ManualControlSettingsData manualSettings;
ManualControlSettingsGet(&manualSettings);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
if (rate) { // rate mode
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
switch(state) {
case AT_INIT:
lastUpdateTime = xTaskGetTickCount();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0)
state = AT_START;
break;
case AT_START:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Spend the first block of time in normal rate mode to get airborne
if (diffTime > PREPARE_TIME) {
state = AT_ROLL;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_ROLL:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the roll axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_PITCH;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_PITCH:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the pitch axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_FINISHED;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_FINISHED:
// Wait until disarmed and landed before updating the settings
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0)
state = AT_SET;
break;
case AT_SET:
update_stabilization_settings();
state = AT_INIT;
break;
default:
// Set an alarm or some shit like that
break;
}
StabilizationDesiredSet(&stabDesired);
vTaskDelay(10);
}
}
int32_t LoggingStart(void)
{
DebugLogSettingsConnectCallback(SettingsUpdatedCb);
DebugLogControlConnectCallback(ControlUpdatedCb);
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);
return 0;
}
MODULE_INITCALL(LoggingInitialize, LoggingStart);
static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots);
DebugLogStatusSet(&status);
}
static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightStatusGet(&flightstatus);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) {
if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled.");
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogSettingsGet(&settings);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("On board logging enabled.");
} else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) {
PIOS_DEBUGLOG_Printf("On board logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
FlightStatusUpdatedCb(NULL);
}
}
static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogControlGet(&control);
if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) {
memset(entry, 0, sizeof(DebugLogEntryData));
if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) {
// reading from log failed, mark as non existent in output
entry->Flight = control.Flight;
entry->Entry = control.Entry;
entry->Type = DEBUGLOGENTRY_TYPE_EMPTY;
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();
}
}
StatusUpdatedCb(ev);
}
void plan_run_AutoTakeoff()
{
StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
switch (autotakeoffState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (!flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
plan_setup_land();
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (!flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
// nothing to do. land has been requested. stay here for forever until mode change.
default:
break;
}
if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
if (priorState != autotakeoffState) {
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
}
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
/* Make sure disarmed on power up */
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
FlightStatusSet(&flightStatus);
// Main task loop
lastSysTime = PIOS_Thread_Systime();
// Select failsafe before run
failsafe_control_select(true);
while (1) {
// Process periodic data for each of the controllers, including reading
// all available inputs
failsafe_control_update();
transmitter_control_update();
tablet_control_update();
geofence_control_update();
// Initialize to invalid value to ensure first update sets FlightStatus
static FlightStatusControlSourceOptions last_control_selection = -1;
enum control_events control_events = CONTROL_EVENTS_NONE;
// Control logic to select the valid controller
FlightStatusControlSourceOptions control_selection = control_source_select();
bool reset_controller = control_selection != last_control_selection;
// This logic would be better collapsed into control_source_select but
// in the case of the tablet we need to be able to abort and fall back
// to failsafe for now
switch(control_selection) {
case FLIGHTSTATUS_CONTROLSOURCE_TRANSMITTER:
transmitter_control_select(reset_controller);
control_events = transmitter_control_get_events();
break;
case FLIGHTSTATUS_CONTROLSOURCE_TABLET:
{
static bool tablet_previously_succeeded = false;
if (tablet_control_select(reset_controller) == 0) {
control_events = tablet_control_get_events();
tablet_previously_succeeded = true;
} else {
// Failure in tablet control. This would be better if done
// at the selection stage before the tablet is even used.
failsafe_control_select(reset_controller || tablet_previously_succeeded);
control_events = failsafe_control_get_events();
tablet_previously_succeeded = false;
}
break;
}
case FLIGHTSTATUS_CONTROLSOURCE_GEOFENCE:
geofence_control_select(reset_controller);
control_events = geofence_control_get_events();
break;
case FLIGHTSTATUS_CONTROLSOURCE_FAILSAFE:
default:
failsafe_control_select(reset_controller);
control_events = failsafe_control_get_events();
break;
}
if (control_selection != last_control_selection) {
FlightStatusControlSourceSet(&control_selection);
last_control_selection = control_selection;
}
// TODO: This can evolve into a full FSM like I2C possibly
switch(control_events) {
case CONTROL_EVENTS_NONE:
break;
case CONTROL_EVENTS_ARM:
control_event_arm();
break;
case CONTROL_EVENTS_ARMING:
control_event_arming();
break;
case CONTROL_EVENTS_DISARM:
control_event_disarm();
break;
}
// Wait until next update
PIOS_Thread_Sleep_Until(&lastSysTime, UPDATE_PERIOD_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
}
}
void FlightStatusControlSource(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
FlightStatusData data;
FlightStatusGet(&data);
ReturnValue->Val->Integer = data.ControlSource;
}
/* library functions */
void FlightStatusArmed(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
FlightStatusData data;
FlightStatusGet(&data);
ReturnValue->Val->Integer = data.Armed;
}
static void loggingTask(void *parameters)
{
UAVObjEvent ev;
bool armed = false;
uint32_t now = PIOS_Thread_Systime();
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
bool write_open = false;
bool read_open = false;
int32_t read_sector = 0;
uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM];
#endif
// Get settings automatically for now on
LoggingSettingsConnectCopy(&settings);
LoggingStatsGet(&loggingData);
loggingData.BytesLogged = 0;
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash)
{
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
}
#endif
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) {
loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING;
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
LoggingStatsSet(&loggingData);
// Loop forever
while (1)
{
LoggingStatsGet(&loggingData);
// Check for change in armed state if logging on armed
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) {
// Start logging because just armed
loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING;
armed = true;
LoggingStatsSet(&loggingData);
} else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
armed = false;
LoggingStatsSet(&loggingData);
}
}
switch (loggingData.Operation) {
case LOGGINGSTATS_OPERATION_FORMAT:
// Format the file system
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash){
if (read_open || write_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
write_open = false;
}
PIOS_STREAMFS_Format(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
LoggingStatsSet(&loggingData);
break;
case LOGGINGSTATS_OPERATION_INITIALIZING:
// Unregister all objects
UAVObjIterate(&unregister_object);
// Register objects to be logged
switch (settings.Profile) {
case LOGGINGSETTINGS_PROFILE_DEFAULT:
register_default_profile();
break;
case LOGGINGSETTINGS_PROFILE_CUSTOM:
UAVObjIterate(®ister_object);
break;
}
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash){
// Close the file if it is open for reading
if (read_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
}
// Open the file if it is not open for writing
if (!write_open) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
continue;
} else {
write_open = true;
}
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
}
}
else {
read_open = false;
write_open = true;
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
// Write information at start of the log file
writeHeader();
// Log settings
if (settings.LogSettingsOnStart == LOGGINGSETTINGS_LOGSETTINGSONSTART_TRUE){
UAVObjIterate(&logSettings);
}
// Empty the queue
while(PIOS_Queue_Receive(logging_queue, &ev, 0))
LoggingStatsBytesLoggedSet(&written_bytes);
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
LoggingStatsSet(&loggingData);
break;
case LOGGINGSTATS_OPERATION_LOGGING:
{
// Sleep between writing
PIOS_Thread_Sleep_Until(&now, LOGGING_PERIOD_MS);
// Log the objects registred to the shared queue
for (int i=0; i<LOGGING_QUEUE_SIZE; i++) {
if (PIOS_Queue_Receive(logging_queue, &ev, 0) == true) {
UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0);
}
else {
break;
}
}
LoggingStatsBytesLoggedSet(&written_bytes);
now = PIOS_Thread_Systime();
}
break;
case LOGGINGSTATS_OPERATION_DOWNLOAD:
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash) {
if (!read_open) {
// Start reading
if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
read_open = true;
read_sector = -1;
}
}
if (read_open && read_sector == loggingData.FileSectorNum) {
// Request received for same sector. Reupdate.
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
} else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) {
int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1);
if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) {
// close on error
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// Check it has really run out of bytes by reading again
int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1);
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// indicate end of file
loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
}
read_sector = loggingData.FileSectorNum;
}
LoggingStatsSet(&loggingData);
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
// fall-through to default case
default:
// Makes sure that we are not hogging the processor
PIOS_Thread_Sleep(10);
#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC)
if (destination_spi_flash) {
// Close the file if necessary
if (write_open) {
PIOS_STREAMFS_Close(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
write_open = false;
}
}
#endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */
}
}
}
/**
* Module thread, should not return.
*/
static void groundPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
GroundPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have GROUND type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, guidanceSettings.UpdatePeriod);
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
static void loggingTask(void *parameters)
{
bool armed = false;
bool write_open = false;
bool read_open = false;
int32_t read_sector = 0;
uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM];
//PIOS_STREAMFS_Format(streamfs_id);
LoggingStatsData loggingData;
LoggingStatsGet(&loggingData);
loggingData.BytesLogged = 0;
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingSettingsData settings;
LoggingSettingsGet(&settings);
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
write_open = false;
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
write_open = true;
}
} else {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
LoggingStatsSet(&loggingData);
int i = 0;
// Loop forever
while (1) {
// Do not update anything at more than 40 Hz
PIOS_Thread_Sleep(20);
LoggingStatsGet(&loggingData);
// Check for change in armed state if logging on armed
LoggingSettingsGet(&settings);
if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) {
// Start logging because just armed
loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING;
armed = true;
LoggingStatsSet(&loggingData);
} else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) {
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
armed = false;
LoggingStatsSet(&loggingData);
}
}
// If currently downloading a log, close the file
if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && read_open) {
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
}
if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && !write_open) {
if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
write_open = true;
}
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
} else if (loggingData.Operation != LOGGINGSTATS_OPERATION_LOGGING && write_open) {
PIOS_STREAMFS_Close(streamfs_id);
loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id);
loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id);
LoggingStatsSet(&loggingData);
write_open = false;
}
switch (loggingData.Operation) {
case LOGGINGSTATS_OPERATION_LOGGING:
if (!write_open)
continue;
UAVTalkSendObjectTimestamped(uavTalkCon, AttitudeActualHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, AccelsHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, GyrosHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, MagnetometerHandle(), 0, false, 0);
if ((i % 10) == 0) {
UAVTalkSendObjectTimestamped(uavTalkCon, BaroAltitudeHandle(), 0, false, 0);
UAVTalkSendObjectTimestamped(uavTalkCon, GPSPositionHandle(), 0, false, 0);
}
if ((i % 50) == 1) {
UAVTalkSendObjectTimestamped(uavTalkCon, GPSTimeHandle(), 0, false, 0);
}
LoggingStatsBytesLoggedSet(&written_bytes);
break;
case LOGGINGSTATS_OPERATION_DOWNLOAD:
if (!read_open) {
// Start reading
if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) {
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
} else {
read_open = true;
read_sector = -1;
}
}
if (read_open && read_sector == loggingData.FileSectorNum) {
// Request received for same sector. Reupdate.
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
} else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) {
int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1);
if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) {
// close on error
loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// Check it has really run out of bytes by reading again
int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1);
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) {
// indicate end of file
loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE;
PIOS_STREAMFS_Close(streamfs_id);
read_open = false;
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
}
} else {
// Indicate sent
loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE;
memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM);
}
read_sector = loggingData.FileSectorNum;
}
LoggingStatsSet(&loggingData);
}
i++;
}
}
static void uavoMavlinkBridgeTask(void *parameters) {
uint32_t lastSysTime;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
FlightBatterySettingsData batSettings = {};
if (FlightBatterySettingsHandle() != NULL )
FlightBatterySettingsGet(&batSettings);
SystemStatsData systemStats;
while (1) {
PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ);
if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
FlightBatteryStateData batState = {};
if (FlightBatteryStateHandle() != NULL )
FlightBatteryStateGet(&batState);
SystemStatsGet(&systemStats);
int8_t battery_remaining = 0;
if (batSettings.Capacity != 0) {
if (batState.ConsumedEnergy < batSettings.Capacity) {
battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100);
}
}
uint16_t voltage = 0;
if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
voltage = lroundf(batState.Voltage * 1000);
uint16_t current = 0;
if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
current = lroundf(batState.Current * 100);
mavlink_msg_sys_status_pack(0, 200, mav_msg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
(uint16_t)systemStats.CPULoad * 10,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
voltage,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
current,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
battery_remaining,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) {
ManualControlCommandData manualState;
FlightStatusData flightStatus;
ManualControlCommandGet(&manualState);
FlightStatusGet(&flightStatus);
SystemStatsGet(&systemStats);
//TODO connect with RSSI object and pass in last argument
mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
manualState.Channel[0],
// chan2_raw RC channel 2 value, in microseconds
manualState.Channel[1],
// chan3_raw RC channel 3 value, in microseconds
manualState.Channel[2],
// chan4_raw RC channel 4 value, in microseconds
manualState.Channel[3],
// chan5_raw RC channel 5 value, in microseconds
manualState.Channel[4],
// chan6_raw RC channel 6 value, in microseconds
manualState.Channel[5],
// chan7_raw RC channel 7 value, in microseconds
manualState.Channel[6],
// chan8_raw RC channel 8 value, in microseconds
manualState.Channel[7],
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
manualState.Rssi);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_POSITION)) {
GPSPositionData gpsPosData = {};
HomeLocationData homeLocation = {};
SystemStatsGet(&systemStats);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (HomeLocationHandle() != NULL )
HomeLocationGet(&homeLocation);
SystemStatsGet(&systemStats);
uint8_t gps_fix_type;
switch (gpsPosData.Status)
{
case GPSPOSITION_STATUS_NOGPS:
gps_fix_type = 0;
break;
case GPSPOSITION_STATUS_NOFIX:
gps_fix_type = 1;
break;
case GPSPOSITION_STATUS_FIX2D:
gps_fix_type = 2;
break;
case GPSPOSITION_STATUS_FIX3D:
case GPSPOSITION_STATUS_DIFF3D:
gps_fix_type = 3;
break;
default:
gps_fix_type = 0;
break;
}
mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
(uint64_t)systemStats.FlightTime * 1000,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gps_fix_type,
// lat Latitude in 1E7 degrees
gpsPosData.Latitude,
// lon Longitude in 1E7 degrees
gpsPosData.Longitude,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsPosData.Altitude * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.HDOP * 100,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.VDOP * 100,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
gpsPosData.Groundspeed * 100,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
gpsPosData.Heading * 100,
// satellites_visible Number of satellites visible. If unknown, set to 255
gpsPosData.Satellites);
send_message();
mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
// latitude Latitude (WGS84), expressed as * 1E7
homeLocation.Latitude,
// longitude Longitude (WGS84), expressed as * 1E7
homeLocation.Longitude,
// altitude Altitude(WGS84), expressed as * 1000
homeLocation.Altitude * 1000);
send_message();
//TODO add waypoint nav stuff
//wp_target_bearing
//wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
//alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
//aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
//xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
//mavlink_msg_nav_controller_output_pack
//wp_number
//mavlink_msg_mission_current_pack
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) {
AttitudeActualData attActual;
SystemStatsData systemStats;
AttitudeActualGet(&attActual);
SystemStatsGet(&systemStats);
mavlink_msg_attitude_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// roll Roll angle (rad)
attActual.Roll * DEG2RAD,
// pitch Pitch angle (rad)
attActual.Pitch * DEG2RAD,
// yaw Yaw angle (rad)
attActual.Yaw * DEG2RAD,
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) {
ActuatorDesiredData actDesired;
AttitudeActualData attActual;
AirspeedActualData airspeedActual = {};
GPSPositionData gpsPosData = {};
BaroAltitudeData baroAltitude = {};
FlightStatusData flightStatus;
if (AirspeedActualHandle() != NULL )
AirspeedActualGet(&airspeedActual);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (BaroAltitudeHandle() != NULL )
BaroAltitudeGet(&baroAltitude);
ActuatorDesiredGet(&actDesired);
AttitudeActualGet(&attActual);
FlightStatusGet(&flightStatus);
float altitude = 0;
if (BaroAltitudeHandle() != NULL)
altitude = baroAltitude.Altitude;
else if (GPSPositionHandle() != NULL)
altitude = gpsPosData.Altitude;
// round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
int16_t heading = lroundf(attActual.Yaw);
if (heading < 0)
heading += 360;
mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
// airspeed Current airspeed in m/s
airspeedActual.TrueAirspeed,
// groundspeed Current ground speed in m/s
gpsPosData.Groundspeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
heading,
// throttle Current throttle setting in integer percent, 0 to 100
actDesired.Throttle * 100,
// alt Current altitude (MSL), in meters
altitude,
// climb Current climb rate in meters/second
0);
send_message();
uint8_t armed_mode = 0;
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED)
armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t custom_mode = CUSTOM_MODE_STAB;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
/* Kinda a catch all */
custom_mode = CUSTOM_MODE_SPORT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ACRO:
case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
custom_mode = CUSTOM_MODE_ACRO;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
/* May want these three to try and
* infer based on roll axis */
case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
custom_mode = CUSTOM_MODE_STAB;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
custom_mode = CUSTOM_MODE_DRIFT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
custom_mode = CUSTOM_MODE_ALTH;
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
custom_mode = CUSTOM_MODE_RTL;
break;
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
custom_mode = CUSTOM_MODE_POSH;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
custom_mode = CUSTOM_MODE_AUTO;
break;
}
mavlink_msg_heartbeat_pack(0, 200, mav_msg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
MAV_TYPE_GENERIC,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC,
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
armed_mode,
// custom_mode A bitfield for use for autopilot-specific flags.
custom_mode,
// system_status System status flag, see MAV_STATE ENUM
0);
send_message();
}
}
}
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit)
{
if (newinit) {
StabilizationDesiredInitialize();
StabilizationBankInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.StabilizationMode.Thrust = stab_settings[3];
stabilization.Thrust = cmd.Thrust;
StabilizationDesiredSet(&stabilization);
}
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
FixedWingAirspeedsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
// Force update of all the settings
SettingsUpdatedCb(NULL);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
path_desired_updated = false;
PathDesiredGet(&pathDesired);
PathDesiredConnectCallback(pathDesiredUpdated);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod);
static uint8_t last_flight_mode;
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PositionActualData positionActual;
static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE;
// Check whether an update to the path desired occured and we should
// process it. This makes sure that the follower alarm state is
// updated.
bool process_path_desired_update =
(last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ||
last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
path_desired_updated;
path_desired_updated = false;
// Process most of these when the flight mode changes
// except when in path following mode in which case
// each iteration must make sure this has the latest
// PathDesired
if (flightStatus.FlightMode != last_flight_mode ||
process_path_desired_update) {
last_flight_mode = flightStatus.FlightMode;
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = 0;
pathDesired.End[1] = 0;
pathDesired.End[2] = -30.0f;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = positionActual.North;
pathDesired.End[1] = positionActual.East;
pathDesired.End[2] = positionActual.Down;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
state = FW_FOLLOWER_RUNNING;
PathDesiredGet(&pathDesired);
switch(pathDesired.Mode) {
case PATHDESIRED_MODE_ENDPOINT:
case PATHDESIRED_MODE_VECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
break;
default:
state = FW_FOLLOWER_ERR;
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
PathStatusSet(&pathStatus);
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
break;
}
break;
default:
state = FW_FOLLOWER_IDLE;
break;
}
}
switch(state) {
case FW_FOLLOWER_RUNNING:
{
updatePathVelocity();
uint8_t result = updateFixedDesiredAttitude();
if (result) {
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(&pathStatus);
break;
}
case FW_FOLLOWER_IDLE:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
bearingIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
airspeedErrorInt = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
break;
case FW_FOLLOWER_ERR:
default:
// Leave alarms set above
break;
}
}
}
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
{
enum AUTOTUNE_STATE state = AT_INIT;
uint32_t last_update_time = PIOS_Thread_Systime();
float X[AF_NUMX] = {0};
float P[AF_NUMP] = {0};
float noise[3] = {0};
af_init(X,P);
uint32_t last_time = 0.0f;
const uint32_t YIELD_MS = 2;
GyrosConnectCallback(at_new_gyro_data);
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
uint32_t diff_time;
const uint32_t PREPARE_TIME = 2000;
const uint32_t MEASURE_TIME = 60000;
static uint32_t update_counter = 0;
bool doing_ident = false;
bool can_sleep = true;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only allow this module to run when autotuning
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
state = AT_INIT;
PIOS_Thread_Sleep(50);
continue;
}
switch(state) {
case AT_INIT:
last_update_time = PIOS_Thread_Systime();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
af_init(X,P);
UpdateSystemIdent(X, NULL, 0.0f, 0, 0);
state = AT_START;
}
break;
case AT_START:
diff_time = PIOS_Thread_Systime() - last_update_time;
// Spend the first block of time in normal rate mode to get airborne
if (diff_time > PREPARE_TIME) {
last_time = PIOS_DELAY_GetRaw();
/* Drain the queue of all current data */
while (circ_queue_read_pos(at_queue)) {
circ_queue_read_completed(at_queue);
}
/* And reset the point spill counter */
update_counter = 0;
at_points_spilled = 0;
state = AT_RUN;
last_update_time = PIOS_Thread_Systime();
}
break;
case AT_RUN:
diff_time = PIOS_Thread_Systime() - last_update_time;
doing_ident = true;
can_sleep = false;
for (int i=0; i<MAX_PTS_PER_CYCLE; i++) {
struct at_queued_data *pt;
/* Grab an autotune point */
pt = circ_queue_read_pos(at_queue);
if (!pt) {
/* We've drained the buffer
* fully. Yay! */
can_sleep = true;
break;
}
/* calculate time between successive
* points */
float dT_s = PIOS_DELAY_DiffuS2(last_time,
pt->raw_time) * 1.0e-6f;
/* This is for the first point, but
* also if we have extended drops */
if (dT_s > 0.010f) {
dT_s = 0.010f;
}
last_time = pt->raw_time;
af_predict(X, P, pt->u, pt->y, dT_s);
// Update uavo every 256 cycles to avoid
// telemetry spam
if (!((update_counter++) & 0xff)) {
UpdateSystemIdent(X, noise, dT_s, update_counter, at_points_spilled);
}
for (uint32_t i = 0; i < 3; i++) {
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
noise[i] = NOISE_ALPHA * noise[i] + (1-NOISE_ALPHA) * (pt->y[i] - X[i]) * (pt->y[i] - X[i]);
}
/* Free the buffer containing an AT point */
circ_queue_read_completed(at_queue);
}
if (diff_time > MEASURE_TIME) { // Move on to next state
state = AT_FINISHED;
last_update_time = PIOS_Thread_Systime();
}
break;
case AT_FINISHED:
// Wait until disarmed and landed before saving the settings
UpdateSystemIdent(X, noise, 0, update_counter, at_points_spilled);
state = AT_WAITING; // Fall through
case AT_WAITING:
// TODO do this unconditionally on disarm,
// no matter what mode we're in.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
// Save the settings locally.
UAVObjSave(SystemIdentHandle(), 0);
state = AT_INIT;
}
break;
default:
// Set an alarm or some shit like that
break;
}
// Update based on manual controls
UpdateStabilizationDesired(doing_ident);
if (can_sleep) {
PIOS_Thread_Sleep(YIELD_MS);
}
}
}
static void FirmwareIAPCallback(UAVObjEvent* ev)
{
const struct pios_board_info * bdinfo = &pios_board_info_blob;
static uint32_t last_time = 0;
uint32_t this_time;
uint32_t delta;
if(iap_state == IAP_STATE_RESETTING)
return;
if ( ev->obj == FirmwareIAPObjHandle() ) {
// Get the input object data
FirmwareIAPObjGet(&data);
this_time = get_time();
delta = this_time - last_time;
last_time = this_time;
if((data.BoardType==bdinfo->board_type)&&(data.crc != PIOS_BL_HELPER_CRC_Memory_Calc()))
{
PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision=bdinfo->board_rev;
data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
FirmwareIAPObjSet( &data );
}
if((data.ArmReset==1)&&(iap_state!=IAP_STATE_RESETTING))
{
data.ArmReset=0;
FirmwareIAPObjSet( &data );
}
switch(iap_state) {
case IAP_STATE_READY:
if( data.Command == IAP_CMD_STEP_1 ) {
iap_state = IAP_STATE_STEP_1;
} break;
case IAP_STATE_STEP_1:
if( data.Command == IAP_CMD_STEP_2 ) {
if( delta > iap_time_2_low_end && delta < iap_time_2_high_end ) {
iap_state = IAP_STATE_STEP_2;
} else {
iap_state = IAP_STATE_READY;
}
} else {
iap_state = IAP_STATE_READY;
}
break;
case IAP_STATE_STEP_2:
if( data.Command == IAP_CMD_STEP_3 ) {
if( delta > iap_time_3_low_end && delta < iap_time_3_high_end ) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
// Abort any attempts if not disarmed
iap_state = IAP_STATE_READY;
break;
}
// we've met the three sequence of command numbers
// we've met the time requirements.
PIOS_IAP_SetRequest1();
PIOS_IAP_SetRequest2();
/* Note: Cant just wait timeout value, because first time is randomized */
reset_count = 0;
lastResetSysTime = xTaskGetTickCount();
UAVObjEvent * ev = pvPortMalloc(sizeof(UAVObjEvent));
memset(ev,0,sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(ev, resetTask, 100);
iap_state = IAP_STATE_RESETTING;
} else {
iap_state = IAP_STATE_READY;
}
} else {
iap_state = IAP_STATE_READY;
}
break;
case IAP_STATE_RESETTING:
// stay here permanentally, should reboot
break;
default:
iap_state = IAP_STATE_READY;
last_time = 0; // Reset the time counter, as we are not doing a IAP reset
break;
}
}
}
/**
* Module thread, should not return.
*/
static void guidanceTask(void *parameters)
{
SystemSettingsData systemSettings;
GuidanceSettingsData guidanceSettings;
FlightStatusData flightStatus;
portTickType thisTime;
portTickType lastUpdateTime;
UAVObjEvent ev;
float accel[3] = {0,0,0};
uint32_t accel_accum = 0;
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
GuidanceSettingsGet(&guidanceSettings);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
// Collect downsampled attitude data
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
accel[0] += attitudeRaw.accels[0];
accel[1] += attitudeRaw.accels[1];
accel[2] += attitudeRaw.accels[2];
accel_accum++;
// Continue collecting data if not enough time
thisTime = xTaskGetTickCount();
if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
continue;
lastUpdateTime = xTaskGetTickCount();
accel[0] /= accel_accum;
accel[1] /= accel_accum;
accel[2] /= accel_accum;
//rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0]=attitudeActual.q1;
q[1]=attitudeActual.q2;
q[2]=attitudeActual.q3;
q[3]=attitudeActual.q4;
Quaternion2R(q, Rbe);
for (uint8_t i=0; i<3; i++){
accel_ned[i]=0;
for (uint8_t j=0; j<3; j++)
accel_ned[i] += Rbe[j][i]*accel[j];
}
accel_ned[2] += 9.81;
NedAccelData accelData;
NedAccelGet(&accelData);
// Convert from m/s to cm/s
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);
FlightStatusGet(&flightStatus);
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) &&
((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
{
if(positionHoldLast == 0) {
/* When enter position hold mode save current position */
PositionDesiredData positionDesired;
PositionActualData positionActual;
PositionDesiredGet(&positionDesired);
PositionActualGet(&positionActual);
positionDesired.North = positionActual.North;
positionDesired.East = positionActual.East;
PositionDesiredSet(&positionDesired);
positionHoldLast = 1;
}
if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD )
updateVtolDesiredVelocity();
else
manualSetDesiredVelocity();
updateVtolDesiredAttitude();
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
positionHoldLast = 0;
}
accel[0] = accel[1] = accel[2] = 0;
accel_accum = 0;
}
}
