/**
* Register objects for the default logging profile
*/
static void register_default_profile()
{
// For the default profile, we limit things to 100Hz (for now)
uint16_t min_period = MAX(get_minimum_logging_period(), 10);
// Objects for which we log all changes (use 100Hz to limit max data rate)
UAVObjConnectCallbackThrottled(FlightStatusHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10);
UAVObjConnectCallbackThrottled(SystemAlarmsHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10);
if (WaypointActiveHandle()) {
UAVObjConnectCallbackThrottled(WaypointActiveHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10);
}
if (SystemIdentHandle()){
UAVObjConnectCallbackThrottled(SystemIdentHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10);
}
// Log fast
UAVObjConnectCallbackThrottled(AccelsHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, min_period);
UAVObjConnectCallbackThrottled(GyrosHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, min_period);
// Log a bit slower
UAVObjConnectCallbackThrottled(AttitudeActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period);
UAVObjConnectCallbackThrottled(MagnetometerHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period);
UAVObjConnectCallbackThrottled(ManualControlCommandHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period);
UAVObjConnectCallbackThrottled(ActuatorDesiredHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period);
UAVObjConnectCallbackThrottled(StabilizationDesiredHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 5 * min_period);
// Log slow
if (FlightBatteryStateHandle()) {
UAVObjConnectCallbackThrottled(FlightBatteryStateHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
if (BaroAltitudeHandle()) {
UAVObjConnectCallbackThrottled(BaroAltitudeHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
if (AirspeedActualHandle()) {
UAVObjConnectCallbackThrottled(AirspeedActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
if (GPSPositionHandle()) {
UAVObjConnectCallbackThrottled(GPSPositionHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
if (PositionActualHandle()) {
UAVObjConnectCallbackThrottled(PositionActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
if (VelocityActualHandle()) {
UAVObjConnectCallbackThrottled(VelocityActualHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 10 * min_period);
}
// Log very slow
if (GPSTimeHandle()) {
UAVObjConnectCallbackThrottled(GPSTimeHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 50 * min_period);
}
// Log very very slow
if (GPSSatellitesHandle()) {
UAVObjConnectCallbackThrottled(GPSSatellitesHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 500 * min_period);
}
}
/**
* Module initialization
*/
int32_t StabilizationStart()
{
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
BankUpdatedCb(StabilizationBankHandle());
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
return 0;
}
void ManualControlCommandChannelGet( uint16_t *NewChannel )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewChannel, offsetof( ManualControlCommandData, Channel), 9*sizeof(uint16_t));
}
void ManualControlCommandConnectedGet( uint8_t *NewConnected )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewConnected, offsetof( ManualControlCommandData, Connected), sizeof(uint8_t));
}
void ManualControlCommandCollectiveGet( float *NewCollective )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewCollective, offsetof( ManualControlCommandData, Collective), sizeof(float));
}
void ManualControlCommandYawGet( float *NewYaw )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewYaw, offsetof( ManualControlCommandData, Yaw), sizeof(float));
}
void ManualControlCommandPitchGet( float *NewPitch )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewPitch, offsetof( ManualControlCommandData, Pitch), sizeof(float));
}
void ManualControlCommandRollGet( float *NewRoll )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewRoll, offsetof( ManualControlCommandData, Roll), sizeof(float));
}
void ManualControlCommandThrottleGet( float *NewThrottle )
{
UAVObjGetDataField(ManualControlCommandHandle(), (void*)NewThrottle, offsetof( ManualControlCommandData, Throttle), sizeof(float));
}