static void UpdateStabilizationDesired(bool doingIdent) {
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
uint8_t rollMax, pitchMax;
float manualRate[STABILIZATIONSETTINGS_MANUALRATE_NUMELEM];
StabilizationSettingsRollMaxGet(&rollMax);
StabilizationSettingsPitchMaxGet(&pitchMax);
StabilizationSettingsManualRateGet(manualRate);
ManualControlCommandRollGet(&stabDesired.Roll);
stabDesired.Roll *= rollMax;
ManualControlCommandPitchGet(&stabDesired.Pitch);
stabDesired.Pitch *= pitchMax;
ManualControlCommandYawGet(&stabDesired.Yaw);
stabDesired.Yaw *= manualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
if (doingIdent) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
ManualControlCommandThrottleGet(&stabDesired.Throttle);
StabilizationDesiredSet(&stabDesired);
}
/**
* @brief execute autocruise
*/
void plan_run_AutoCruise()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float controlVector[4];
ManualControlCommandRollGet(&controlVector[0]);
ManualControlCommandPitchGet(&controlVector[1]);
ManualControlCommandYawGet(&controlVector[2]);
controlVector[3] = 0.5f; // dummy, thrust is normalized separately
normalizeDeadband(controlVector); // return value ignored
ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
// normalize old desired movement vector
float vector[3] = { pathDesired.End.North - hold_position[0],
pathDesired.End.East - hold_position[1],
pathDesired.End.Down - hold_position[2] };
float length = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
if (length < 1e-9f) {
length = 1.0f; // should not happen since initialized properly in setup()
}
vector[0] /= length;
vector[1] /= length;
vector[2] /= length;
// start position is advanced according to actual movement - in the direction of desired vector only
// projection using scalar product
float kp = (positionState.North - hold_position[0]) * vector[0]
+ (positionState.East - hold_position[1]) * vector[1]
+ (positionState.Down - hold_position[2]) * vector[2];
if (kp > 0.0f) {
hold_position[0] += kp * vector[0];
hold_position[1] += kp * vector[1];
hold_position[2] += kp * vector[2];
}
// new angle is equal to old angle plus offset depending on yaw input and time
// (controlVector is normalized with a deadband, change is zero within deadband)
float angle = RAD2DEG(atan2f(vector[1], vector[0]));
float dT = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
angle += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
// resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
pathDesired.End.North = hold_position[0] + vector[0];
pathDesired.End.East = hold_position[1] + vector[1];
pathDesired.End.Down = hold_position[2] + vector[2];
// start position has the same offset as in position hold
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
PathDesiredSet(&pathDesired);
}
static void plan_run_PositionVario(vario_type type)
{
float controlVector[4];
float alpha;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
ManualControlCommandRollGet(&controlVector[0]);
ManualControlCommandPitchGet(&controlVector[1]);
ManualControlCommandYawGet(&controlVector[2]);
ManualControlCommandThrustGet(&controlVector[3]);
FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
controlVector[0] = vario_control_lowpass[0];
controlVector[1] = vario_control_lowpass[1];
controlVector[2] = vario_control_lowpass[2];
// check if movement is desired
if (normalizeDeadband(controlVector) == false) {
// no movement desired, re-enter positionHold at current start-position
if (!vario_hold) {
vario_hold = true;
// new hold position is the position that was previously the start position
pathDesired.End.North = hold_position[0];
pathDesired.End.East = hold_position[1];
pathDesired.End.Down = hold_position[2];
// while the new start position has the same offset as in position hold
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
PathDesiredSet(&pathDesired);
}
} else {
PositionStateData positionState;
PositionStateGet(&positionState);
// flip pitch to have pitch down (away) point north
controlVector[1] = -controlVector[1];
getVector(controlVector, type);
// layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4}
if (vario_hold) {
// start position is the position that was previously the hold position
vario_hold = false;
hold_position[0] = pathDesired.End.North;
hold_position[1] = pathDesired.End.East;
hold_position[2] = pathDesired.End.Down;
} else {
// start position is advanced according to movement - in the direction of ControlVector only
// projection using scalar product
float kp = (positionState.North - hold_position[0]) * controlVector[0]
+ (positionState.East - hold_position[1]) * controlVector[1]
+ (positionState.Down - hold_position[2]) * -controlVector[2];
if (kp > 0.0f) {
hold_position[0] += kp * controlVector[0];
hold_position[1] += kp * controlVector[1];
hold_position[2] += kp * -controlVector[2];
}
}
// new destination position is advanced based on controlVector
pathDesired.End.North = hold_position[0] + controlVector[0] * controlVector[3];
pathDesired.End.East = hold_position[1] + controlVector[1] * controlVector[3];
pathDesired.End.Down = hold_position[2] - controlVector[2] * controlVector[3];
// the new start position has the same offset as in position hold
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
PathDesiredSet(&pathDesired);
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
float downVel = velocityActual.Down;
// Compute desired north command from velocity error
northError = velocityDesired.North - northVel;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward;
// Compute desired east command from velocity error
eastError = velocityDesired.East - eastVel;
eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward;
// Compute desired down command. Using NED accel as the damping term
downError = velocityDesired.Down - downVel;
// Negative is critical here since throttle is negative with down
downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) +
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD];
// If this setting is zero then the throttle level available when enabled is used for hover:wq
float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle;
stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw.
// For this to behave well the craft should move similarly for 5 deg roll versus 5 deg pitch.
// Notice the inputs are crudely bounded by the anti-winded but if both N and E were
// saturated and the craft were at 45 degrees that would result in a value greater than
// the limit, so apply limit again here.
stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) +
-eastCommand * sinf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) +
eastCommand * cosf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
float yaw;
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
* @param[in] dT the time since last evaluation
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
int32_t vtol_follower_control_attitude(float dT)
{
vtol_follower_control_accel(dT);
AccelDesiredData accelDesired;
AccelDesiredGet(&accelDesired);
StabilizationDesiredData stabDesired;
float northCommand = accelDesired.North;
float eastCommand = accelDesired.East;
// Project the north and east acceleration signals into body frame
float yaw;
AttitudeActualYawGet(&yaw);
float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD);
float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD);
// Set the angle that would achieve the desired acceleration given the thrust is enough for a hover
stabDesired.Pitch = bound_min_max(RAD2DEG * atanf(forward_accel_desired / GRAVITY),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(RAD2DEG * atanf(right_accel_desired / GRAVITY),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
// Calculate the throttle setting or use pass through from transmitter
if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
ManualControlCommandThrottleGet(&stabDesired.Throttle);
} else {
float downCommand = accelDesired.Down;
AltitudeHoldStateData altitudeHoldState;
altitudeHoldState.VelocityDesired = downCommand;
altitudeHoldState.Integral = vtol_pids[DOWN_VELOCITY].iAccumulator / 1000.0f;
altitudeHoldState.AngleGain = 1.0f;
if (altitudeHoldSettings.AttitudeComp > 0) {
// Throttle desired is at this point the mount desired in the up direction, we can
// account for the attitude if desired
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
// Project a unit vector pointing up into the body frame and
// get the z component
float fraction = attitudeActual.q1 * attitudeActual.q1 -
attitudeActual.q2 * attitudeActual.q2 -
attitudeActual.q3 * attitudeActual.q3 +
attitudeActual.q4 * attitudeActual.q4;
// Add ability to scale up the amount of compensation to achieve
// level forward flight
fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f);
// Dividing by the fraction remaining in the vertical projection will
// attempt to compensate for tilt. This acts like the thrust is linear
// with the output which isn't really true. If the fraction is starting
// to go negative we are inverted and should shut off throttle
downCommand = (fraction > 0.1f) ? (downCommand / fraction) : 0.0f;
altitudeHoldState.AngleGain = 1.0f / fraction;
}
altitudeHoldState.Throttle = downCommand;
AltitudeHoldStateSet(&altitudeHoldState);
stabDesired.Throttle = bound_min_max(downCommand, 0, 1);
}
// Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine
// grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here
float manual_rate[STABILIZATIONSETTINGS_MANUALRATE_NUMELEM];
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
StabilizationSettingsManualRateGet(manual_rate);
stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
StabilizationSettingsManualRateGet(manual_rate);
stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
{
uint8_t yaw_max;
StabilizationSettingsYawMaxGet(&yaw_max);
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = yaw_max * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH:
{
// Face forward on the path
VelocityDesiredData velocityDesired;
VelocityDesiredGet(&velocityDesired);
float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North;
float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG;
if (total_vel2 > 1) {
stabDesired.Yaw = path_direction;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.Yaw = 0;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
return 0;
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
// FlightTelemetryStatsData flightStats;
// GCSTelemetryStatsData gcsTelemetryStatsData;
// int32_t retries;
// int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
}
mavlink_message_t msg;
mavlink_system.sysid = 20;
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.type = MAV_TYPE_FIXED_WING;
uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT;
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
// Setup type and object id fields
uint32_t objId = UAVObjGetID(ev->obj);
// uint64_t timeStamp = 0;
switch(objId) {
case BAROALTITUDE_OBJID:
{
BaroAltitudeGet(&baroAltitude);
pressure.press_abs = baroAltitude.Pressure*10.0f;
pressure.temperature = baroAltitude.Temperature*100.0f;
mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case FLIGHTTELEMETRYSTATS_OBJID:
{
// FlightTelemetryStatsData flightTelemetryStats;
FlightTelemetryStatsGet(&flightStats);
// XXX this is a hack to make it think it got a confirmed
// connection
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
GCSTelemetryStatsGet(&gcsTelemetryStatsData);
gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
//
//
// //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass);
// mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case SYSTEMSTATS_OBJID:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t system_state = MAV_STATE_UNINIT;
uint8_t base_mode = 0;
uint8_t custom_mode = 0;
// Set flight mode
switch (flightStatus.FlightMode)
{
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
default:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
}
// Set arming state
switch (flightStatus.Armed)
{
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
system_state = MAV_STATE_ACTIVE;
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case FLIGHTSTATUS_ARMED_DISARMED:
system_state = MAV_STATE_STANDBY;
base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED;
break;
}
// Set HIL
if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state);
SystemStatsData stats;
SystemStatsGet(&stats);
FlightBatteryStateData flightBatteryData;
FlightBatteryStateGet(&flightBatteryData);
FlightBatterySettingsData flightBatterySettings;
FlightBatterySettingsGet(&flightBatterySettings);
uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f);
int16_t batteryCurrent = -1; // -1: Not present / not estimated
int8_t batteryPercent = -1; // -1: Not present / not estimated
// if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0)
// {
// Factor is zero, sensor is not present
// Estimate remaining capacity based on lipo curve
batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f));
// }
// else
// {
// // Use capacity and current
// batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity);
// batteryCurrent = flightBatteryData.Current*100;
// }
mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case ATTITUDERAW_OBJID:
{
AttitudeRawGet(&attitudeRaw);
// Copy data
attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X];
attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y];
attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z];
attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X];
attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y];
attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z];
attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X];
attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y];
attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z];
mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
if (hilEnabled)
{
mavlink_hil_controls_t controls;
// Copy data
controls.roll_ailerons = 0.1;
controls.pitch_elevator = 0.1;
controls.yaw_rudder = 0.0;
controls.throttle = 0.8;
mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
}
case ATTITUDEMATRIX_OBJID:
{
AttitudeMatrixGet(&attitudeMatrix);
// Copy data
attitude.roll = attitudeMatrix.Roll;
attitude.pitch = attitudeMatrix.Pitch;
attitude.yaw = attitudeMatrix.Yaw;
attitude.rollspeed = attitudeMatrix.AngularRates[0];
attitude.pitchspeed = attitudeMatrix.AngularRates[1];
attitude.yawspeed = attitudeMatrix.AngularRates[2];
mavlink_msg_attitude_encode(mavlink_system.sysid,
mavlink_system.compid, &msg, &attitude);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case GPSPOSITION_OBJID:
{
GPSPositionGet(&gpsPosition);
gps_raw.time_usec = 0;
gps_raw.lat = gpsPosition.Latitude*10;
gps_raw.lon = gpsPosition.Longitude*10;
gps_raw.alt = gpsPosition.Altitude*10;
gps_raw.eph = gpsPosition.HDOP*100;
gps_raw.epv = gpsPosition.VDOP*100;
gps_raw.cog = gpsPosition.Heading*100;
gps_raw.satellites_visible = gpsPosition.Satellites;
gps_raw.fix_type = gpsPosition.Status;
mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
// mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0);
break;
}
case POSITIONACTUAL_OBJID:
{
PositionActualData pos;
PositionActualGet(&pos);
mavlink_local_position_ned_t m_pos;
m_pos.time_boot_ms = 0;
m_pos.x = pos.North;
m_pos.y = pos.East;
m_pos.z = pos.Down;
m_pos.vx = 0.0f;
m_pos.vy = 0.0f;
m_pos.vz = 0.0f;
mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
case ACTUATORCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
ActuatorCommandData act;
ActuatorCommandGet(&act);
rc.chan5_scaled = act.Channel[0];
rc.chan6_scaled = act.Channel[1];
rc.chan7_scaled = act.Channel[2];
rc.chan8_scaled = act.Channel[3];
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
case MANUALCONTROLCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
rc.chan5_scaled = 0;
rc.chan6_scaled = 0;
rc.chan7_scaled = 0;
rc.chan8_scaled = 0;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
default:
{
//printf("unknown object: %x\n",(unsigned int)objId);
break;
}
}
}
}
/**
* Compute desired attitude from the desired velocity
* @param[in] dT the time since last evaluation
* @param[in] att_adj an adjustment to the attitude for loiter mode
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
int32_t vtol_follower_control_attitude(float dT, const float *att_adj)
{
vtol_follower_control_accel(dT);
float default_adj[2] = {0,0};
if (!att_adj) {
att_adj = default_adj;
}
AccelDesiredData accelDesired;
AccelDesiredGet(&accelDesired);
StabilizationSettingsData stabSet;
StabilizationSettingsGet(&stabSet);
float northCommand = accelDesired.North;
float eastCommand = accelDesired.East;
// Project the north and east acceleration signals into body frame
float yaw;
AttitudeActualYawGet(&yaw);
float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD);
float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD);
StabilizationDesiredData stabDesired;
// Set the angle that would achieve the desired acceleration given the thrust is enough for a hover
stabDesired.Pitch = bound_sym(RAD2DEG * atanf(forward_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[1];
stabDesired.Roll = bound_sym(RAD2DEG * atanf(right_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[0];
// Re-bound based on maximum attitude settings
stabDesired.Pitch = bound_sym(stabDesired.Pitch, stabSet.PitchMax);
stabDesired.Roll = bound_sym(stabDesired.Roll, stabSet.RollMax);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
// Calculate the throttle setting or use pass through from transmitter
if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
ManualControlCommandThrottleGet(&stabDesired.Throttle);
} else {
float downCommand = vtol_follower_control_altitude(accelDesired.Down);
stabDesired.Throttle = bound_min_max(downCommand, 0, 1);
}
// Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine
// grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
{
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH:
{
// Face forward on the path
VelocityDesiredData velocityDesired;
VelocityDesiredGet(&velocityDesired);
float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North;
float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG;
if (total_vel2 > 1) {
stabDesired.Yaw = path_direction;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.Yaw = 0;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
return 0;
}
