/* void sync(int): synchronize an interval by given ms-value */
void SystemSync(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
static uint32_t lastSysTime;
if ((lastSysTime == 0) || (Param[0]->Val->Integer == 0)) {
lastSysTime = PIOS_Thread_Systime();
}
if (Param[0]->Val->Integer > 0) {
PIOS_Thread_Sleep_Until(&lastSysTime, Param[0]->Val->Integer);
}
}
void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, uint32_t *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
//Wait until our turn.
PIOS_Thread_Sleep_Until(lastSysTime, SAMPLING_DELAY_MS_ETASV3);
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
//Check to see if airspeed sensor is returning baroAirspeedData
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (baroAirspeedData->SensorValue == -1) {
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
baroAirspeedData->CalibratedAirspeed = 0;
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_COUNT_IDLE) {
calibrationCount++;
calibrationSum = 0;
return;
} else if (calibrationCount < CALIBRATION_COUNT_IDLE + CALIBRATION_COUNT) {
calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue;
if (calibrationCount == CALIBRATION_COUNT_IDLE + CALIBRATION_COUNT) {
airspeedSettingsData.ZeroPoint = (uint16_t) roundf(((float)calibrationSum) / CALIBRATION_COUNT);
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );
} else {
return;
}
}
//Compute airspeed
float calibratedAirspeed;
if (baroAirspeedData->SensorValue > airspeedSettingsData.ZeroPoint)
calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint);
else
calibratedAirspeed = 0;
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
}
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
{
ExampleSettingsData settings;
ExampleObject2Data data;
int32_t step;
uint32_t lastSysTime;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
while (1) {
// Update settings with latest value
ExampleSettingsGet(&settings);
// Get the object data
ExampleObject2Get(&data);
// Determine how to update the data
if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) {
step = settings.StepSize;
} else {
step = -settings.StepSize;
}
// Update the data
data.Field1 += step;
data.Field2 += step;
data.Field3 += step;
data.Field4[0] += step;
data.Field4[1] += step;
// Update the ExampleObject, after this function is called
// notifications to any other modules listening to that object
// will be sent and the GCS object will be updated through the
// telemetry link. All operations will take place asynchronously
// and the following call will return immediately.
ExampleObject2Set(&data);
// Since this module executes at fixed time intervals, we need to
// block the task until it is time for the next update.
PIOS_Thread_Sleep_Until(&lastSysTime, settings.UpdatePeriod);
}
}
/**
* Called whenver the PWM output timer wraps around which is quite frequenct (e.g. 30khz) to
* update the phase on the outputs based on the current rate
*/
static void PIOS_BRUSHLESS_Task(void* parameters)
{
const uint32_t TICK_DELAY = 1;
uint32_t lastSysTime = PIOS_Thread_Systime();
while (1) {
PIOS_Thread_Sleep_Until(&lastSysTime, TICK_DELAY);
const float dT = TICK_DELAY * 0.001f;
if (!locked) {
for (int channel = 0; channel < NUM_BGC_CHANNELS; channel++) {
// Update phase and keep within [0 360)
phases[channel] += speeds[channel] * dT;
if (phases[channel] < 0)
phases[channel] += 360;
if (phases[channel] >= 360)
phases[channel] -= 360;
PIOS_Brushless_SetPhase(channel, phases[channel] + phase_lag[channel]);
}
} else {
for (int channel = 0; channel < NUM_BGC_CHANNELS; channel++) {
// Update phase and keep within [0 360)
if (phases[channel] < 0)
phases[channel] += 360;
if (phases[channel] >= 360)
phases[channel] -= 360;
PIOS_Brushless_SetPhase(channel, phases[channel]);
}
}
}
}
/**
* 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;
}
}
}
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();
}
}
}
/**
* 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)
{
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 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 baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, uint32_t *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
/* Do nothing when driver support not compiled. */
PIOS_Thread_Sleep_Until(lastSysTime, 1000);
}