static void updateSettings()
{
if (mavlink_port) {
// Retrieve settings
uint8_t speed;
ModuleSettingsMavlinkSpeedGet(&speed);
// Set port speed
switch (speed) {
case MODULESETTINGS_MAVLINKSPEED_2400:
PIOS_COM_ChangeBaud(mavlink_port, 2400);
break;
case MODULESETTINGS_MAVLINKSPEED_4800:
PIOS_COM_ChangeBaud(mavlink_port, 4800);
break;
case MODULESETTINGS_MAVLINKSPEED_9600:
PIOS_COM_ChangeBaud(mavlink_port, 9600);
break;
case MODULESETTINGS_MAVLINKSPEED_19200:
PIOS_COM_ChangeBaud(mavlink_port, 19200);
break;
case MODULESETTINGS_MAVLINKSPEED_38400:
PIOS_COM_ChangeBaud(mavlink_port, 38400);
break;
case MODULESETTINGS_MAVLINKSPEED_57600:
PIOS_COM_ChangeBaud(mavlink_port, 57600);
break;
case MODULESETTINGS_MAVLINKSPEED_115200:
PIOS_COM_ChangeBaud(mavlink_port, 115200);
break;
}
}
}
static void updateSettings()
{
if (usart_port) {
// Retrieve settings
uint8_t speed;
ModuleSettingsComUsbBridgeSpeedGet(&speed);
// Set port speed
switch (speed) {
case MODULESETTINGS_COMUSBBRIDGESPEED_2400:
PIOS_COM_ChangeBaud(usart_port, 2400);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_4800:
PIOS_COM_ChangeBaud(usart_port, 4800);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_9600:
PIOS_COM_ChangeBaud(usart_port, 9600);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_19200:
PIOS_COM_ChangeBaud(usart_port, 19200);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_38400:
PIOS_COM_ChangeBaud(usart_port, 38400);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_57600:
PIOS_COM_ChangeBaud(usart_port, 57600);
break;
case MODULESETTINGS_COMUSBBRIDGESPEED_115200:
PIOS_COM_ChangeBaud(usart_port, 115200);
break;
}
}
}
/**
* Update the telemetry settings, called on startup.
* FIXME: This should be in the TelemetrySettings object. But objects
* have too much overhead yet. Also the telemetry has no any specific
* settings, etc. Thus the HwSettings object which contains the
* telemetry port speed is used for now.
*/
static void updateSettings()
{
if (telemetryPort) {
// Retrieve settings
uint8_t speed;
HwSettingsTelemetrySpeedGet(&speed);
// Set port speed
switch (speed) {
case HWSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(telemetryPort, 2400);
break;
case HWSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(telemetryPort, 4800);
break;
case HWSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(telemetryPort, 9600);
break;
case HWSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(telemetryPort, 19200);
break;
case HWSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(telemetryPort, 38400);
break;
case HWSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(telemetryPort, 57600);
break;
case HWSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(telemetryPort, 115200);
break;
}
}
}
/**
* Initialize the module
* \return -1 if initialization failed
* \return 0 on success
*/
static int32_t uavoTaranisInitialize(void)
{
uint32_t sport_com = PIOS_COM_FRSKY_SPORT;
if (sport_com) {
frsky = PIOS_malloc(sizeof(struct frsky_sport_telemetry));
if (frsky != NULL) {
memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry));
// These objects are registered on the TLM so it
// can intercept them from the telemetry stream
FlightBatteryStateInitialize();
FlightStatusInitialize();
PositionActualInitialize();
VelocityActualInitialize();
frsky->frsky_settings.use_current_sensor = false;
frsky->frsky_settings.batt_cell_count = 0;
frsky->frsky_settings.use_baro_sensor = false;
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
frsky->last_poll_time = PIOS_DELAY_GetuS();
frsky->ignore_rx_chars = 0;
frsky->scheduled_item = -1;
frsky->com = sport_com;
uint8_t i;
for (i = 0; i < NELEMENTS(frsky_value_items); i++)
frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);
module_enabled = true;
return 0;
}
module_enabled = true;
return 0;
}
module_enabled = false;
return -1;
}
/**
* Update the telemetry settings, called on startup and
* each time the settings object is updated
*/
static void updateSettings()
{
// Set port
telemetryPort = PIOS_COM_TELEM_RF;
// Retrieve settings
TelemetrySettingsGet(&settings);
if (telemetryPort) {
// Set port speed
if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200);
}
}
static void onTimer(UAVObjEvent* ev)
{
static portTickType lastSysTime;
static bool firstRun = true;
static FlightBatteryStateData flightBatteryData;
if (firstRun) {
#ifdef ENABLE_DEBUG_MSG
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
#endif
lastSysTime = xTaskGetTickCount();
//FlightBatteryStateGet(&flightBatteryData);
firstRun = false;
}
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
portTickType thisSysTime;
FlightBatterySettingsData batterySettings;
static float dT = SAMPLE_PERIOD_MS / 1000;
float Bob;
float energyRemaining;
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f);
//lastSysTime = thisSysTime;
FlightBatterySettingsGet(&batterySettings);
//calculate the battery parameters
flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps
Bob =dT; // FIXME: something funky happens if I don't do this... Andrew
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh
if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
flightBatteryData.AvgCurrent=(flightBatteryData.AvgCurrent*0.8)+(flightBatteryData.Current*0.2); //in Amps
//sanity checks
if (flightBatteryData.AvgCurrent<0)flightBatteryData.AvgCurrent=0.0;
if (flightBatteryData.PeakCurrent<0)flightBatteryData.PeakCurrent=0.0;
if (flightBatteryData.ConsumedEnergy<0)flightBatteryData.ConsumedEnergy=0.0;
energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
flightBatteryData.EstimatedFlightTime = ((energyRemaining / (flightBatteryData.AvgCurrent*1000.0))*3600.0);//in Sec
//generate alarms where needed...
if ((flightBatteryData.Voltage<=0)&&(flightBatteryData.Current<=0))
{
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
}
else
{
if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.EstimatedFlightTime < 60) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
// FIXME: should make the battery voltage detection dependent on battery type.
if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
}
lastSysTime = thisSysTime;
FlightBatteryStateSet(&flightBatteryData);
}
/* void ChangeBaud(long): changes the speed of picoc serial port */
void SystemChangeBaud(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if ((PIOS_COM_PICOC) && (Param[0]->Val->UnsignedLongInteger > 0)) {
PIOS_COM_ChangeBaud(PIOS_COM_PICOC, Param[0]->Val->UnsignedLongInteger);
}
}
//! Make sure the GPS is set to the same baudrate as the port
void ubx_cfg_set_baudrate(uintptr_t gps_port, ModuleSettingsGPSSpeedOptions baud_rate)
{
// UBX,41 msg
// 1 - portID
// 0007 - input protocol (all)
// 0001 - output protocol (ubx only)
// 0 - no attempt to autobaud
// number - baudrate
// *XX - checksum
const char * msg_2400 = "$PUBX,41,1,0007,0001,2400,0*1B\r\n";
const char * msg_4800 = "$PUBX,41,1,0007,0001,4800,0*11\r\n";
const char * msg_9600 = "$PUBX,41,1,0007,0001,9600,0*12\r\n";
const char * msg_19200 = "$PUBX,41,1,0007,0001,19200,0*27\r\n";
const char * msg_38400 = "$PUBX,41,1,0007,0001,38400,0*22\r\n";
const char * msg_57600 = "$PUBX,41,1,0007,0001,57600,0*29\r\n";
const char * msg_115200 = "$PUBX,41,1,0007,0001,115200,0*1A\r\n";
const char * msg_230400 = "$PUBX,41,1,0007,0001,230400,0*18\r\n";
const char *msg;
uint32_t baud;
switch (baud_rate) {
case MODULESETTINGS_GPSSPEED_2400:
msg = msg_2400;
baud = 2400;
break;
case MODULESETTINGS_GPSSPEED_4800:
msg = msg_4800;
baud = 4800;
break;
case MODULESETTINGS_GPSSPEED_9600:
msg = msg_9600;
baud = 9600;
break;
case MODULESETTINGS_GPSSPEED_19200:
msg = msg_19200;
baud = 19200;
break;
case MODULESETTINGS_GPSSPEED_38400:
msg = msg_38400;
baud = 38400;
break;
default:
case MODULESETTINGS_GPSSPEED_57600:
msg = msg_57600;
baud = 57600;
break;
case MODULESETTINGS_GPSSPEED_115200:
msg = msg_115200;
baud = 115200;
break;
case MODULESETTINGS_GPSSPEED_230400:
msg = msg_230400;
baud = 230400;
break;
}
// Attempt to set baud rate to desired value from a number of
// common rates. So this configures the physical baudrate and
// tries to send the configuration string to the GPS.
const uint32_t baud_rates[] = {2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400};
for (uint32_t i = 0; i < NELEMENTS(baud_rates); i++) {
PIOS_COM_ChangeBaud(gps_port, baud_rates[i]);
PIOS_Thread_Sleep(UBLOX_WAIT_MS);
// Send the baud rate change message
PIOS_COM_SendString(gps_port, msg);
// Wait until the message has been fully transmitted including all start+stop bits
// 34 bytes * 10bits/byte = 340 bits
// At 2400bps, that's (340 / 2400) = 142ms
// add some margin and we end up with 200ms
PIOS_Thread_Sleep(200);
}
// Set to proper baud rate
PIOS_COM_ChangeBaud(gps_port, baud);
}
