/**
* 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();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* 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);
}
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
int32_t accel_raw[3], gyro_raw[3];
uint16_t i;
AHRSCalibrationData calibration;
AHRSCalibrationGet( &calibration );
// Get the Y data. Third byte in. Convert to m/s
accel_raw[0] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[0] +=
( valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] +
calibration.accel_bias[1] ) * fir_coeffs[i];
accel_data.filtered.y =
( float )accel_raw[0] / ( float )fir_coeffs[adc_oversampling] *
calibration.accel_scale[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_raw[1] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[1] +=
( valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] +
calibration.accel_bias[0] ) * fir_coeffs[i];
accel_data.filtered.x =
( float )accel_raw[1] / ( float )fir_coeffs[adc_oversampling] *
calibration.accel_scale[0];
// Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[2] +=
( valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] +
calibration.accel_bias[2] ) * fir_coeffs[i];
accel_data.filtered.z =
-( float )accel_raw[2] / ( float )fir_coeffs[adc_oversampling] *
calibration.accel_scale[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[0] +=
( valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] +
calibration.gyro_bias[0] ) * fir_coeffs[i];
gyro_data.filtered.x =
( float )gyro_raw[0] / ( float )fir_coeffs[adc_oversampling] *
calibration.gyro_scale[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[1] +=
( valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] +
calibration.gyro_bias[1] ) * fir_coeffs[i];
gyro_data.filtered.y =
( float )gyro_raw[1] / ( float )fir_coeffs[adc_oversampling] *
calibration.gyro_scale[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[2] +=
( valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] +
calibration.gyro_bias[2] ) * fir_coeffs[i];
gyro_data.filtered.z =
( float )gyro_raw[2] / ( float )fir_coeffs[adc_oversampling] *
calibration.gyro_scale[2];
attitude_raw.gyros_filtered[0] = ( float )( valid_data_buffer[1] + calibration.gyro_bias[0] ) * calibration.gyro_scale[0];
attitude_raw.gyros_filtered[1] = ( float )( valid_data_buffer[3] + calibration.gyro_bias[1] ) * calibration.gyro_scale[1];
attitude_raw.gyros_filtered[2] = ( float )( valid_data_buffer[5] + calibration.gyro_bias[2] ) * calibration.gyro_scale[2];
/*
attitude_raw.gyros_filtered[0] = gyro_data.filtered.x;
attitude_raw.gyros_filtered[1] = gyro_data.filtered.y;
attitude_raw.gyros_filtered[2] = gyro_data.filtered.z;
attitude_raw.accels_filtered[0] = accel_data.filtered.x;
attitude_raw.accels_filtered[1] = accel_data.filtered.y;
attitude_raw.accels_filtered[2] = accel_data.filtered.z;*/
AttitudeRawSet( &attitude_raw );
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
uint8_t c;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;
// Block until a byte is available
PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);
// And process it
if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
{
// Handle packet with waypoint component
mavlink_wpm_message_handler(&rx_msg);
// Handle packet with parameter component
mavlink_pm_message_handler(mavlink_chan, &rx_msg);
switch (rx_msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Check if this is the gcs
mavlink_heartbeat_t beat;
mavlink_msg_heartbeat_decode(&rx_msg, &beat);
if (beat.type == MAV_TYPE_GCS)
{
// Got heartbeat from the GCS, we're good!
lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
break;
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&rx_msg, &mode);
// Check if this system should change the mode
if (mode.target_system == mavlink_system.sysid)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (mode.base_mode)
{
case MAV_MODE_MANUAL_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_MANUAL_DISARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_PREFLIGHT:
{
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_STABILIZE_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_GUIDED_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_AUTO_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
}
bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
if (newHilEnabled != hilEnabled)
{
if (newHilEnabled)
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READONLY;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
}
else
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READWRITE;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
}
}
hilEnabled = newHilEnabled;
FlightStatusSet(&flightStatus);
// Check HIL
bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
enableHil(hilEnabled);
}
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
{
if (hilEnabled)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&rx_msg, &hil);
// Write GPSPosition
GPSPositionData gps;
GPSPositionGet(&gps);
gps.Altitude = hil.alt/10;
gps.Latitude = hil.lat/10;
gps.Longitude = hil.lon/10;
GPSPositionSet(&gps);
// Write PositionActual
PositionActualData pos;
PositionActualGet(&pos);
// FIXME WRITE POSITION HERE
PositionActualSet(&pos);
// Write AttitudeActual
AttitudeActualData att;
AttitudeActualGet(&att);
att.Roll = hil.roll;
att.Pitch = hil.pitch;
att.Yaw = hil.yaw;
// FIXME
//att.RollSpeed = hil.rollspeed;
//att.PitchSpeed = hil.pitchspeed;
//att.YawSpeed = hil.yawspeed;
// Convert to quaternion formulation
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Write AttitudeActual
AttitudeActualSet(&att);
// Write AttitudeRaw
AttitudeRawData raw;
AttitudeRawGet(&raw);
raw.gyros[0] = hil.rollspeed;
raw.gyros[1] = hil.pitchspeed;
raw.gyros[2] = hil.yawspeed;
raw.accels[0] = hil.xacc;
raw.accels[1] = hil.yacc;
raw.accels[2] = hil.zacc;
// raw.magnetometers[0] = hil.xmag;
// raw.magnetometers[0] = hil.ymag;
// raw.magnetometers[0] = hil.zmag;
AttitudeRawSet(&raw);
}
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// FIXME Implement
}
break;
}
}
}
}
