static void radio_make_telem_string(char* buf, size_t len)
{
int n = 0;
n += chsnprintf(buf, len,
"UUU\r\n\r\n AD6AM AD6AM %02d:%02d:%02d %.5f,%.5f (%s, %d)"
" %dm %dm/s\r\n",
M2RStatus.latest.gps_t_hour, M2RStatus.latest.gps_t_min,
M2RStatus.latest.gps_t_sec,
(double)M2RStatus.latest.gps_lat*(double)1e-7f,
(double)M2RStatus.latest.gps_lng*(double)1e-7f,
GPSFixTypes[M2RStatus.latest.gps_fix_type],
M2RStatus.latest.gps_num_sv,
(int)M2FCNoseStatus.latest.se_h,
(int)M2FCNoseStatus.latest.se_v);
n += chsnprintf(buf+n, len-n, "M2R: ");
n += m2status_write_status_summary(&M2RStatus, buf+n, len-n);
/*
n += chsnprintf(buf+n, len-n, "\r\nM2FCBody (%s): ",
StateNames[M2FCBodyStatus.latest.mc_state]);
n += m2status_write_status_summary(&M2FCBodyStatus, buf+n, len-n);
*/
n += chsnprintf(buf+n, len-n, "\r\nM2FCNose (%s): ",
StateNames[M2FCNoseStatus.latest.mc_state]);
n += m2status_write_status_summary(&M2FCNoseStatus, buf+n, len-n);
}
SDRESULT microsd_open_file_inc(FIL* fp, const char* path, const char* ext, SDFS* sd) {
/* File System Return Code */
SDRESULT sderr;
SDMODE mode = FA_WRITE | FA_CREATE_NEW;
/* Buffer to Hold Filename */
char fname[25];
uint32_t file_idx = 0;
/* Continually Re-attempt SD Card Initilisation */
microsd_card_try_init(sd);
while (true) {
/* Attempt to Open File <fname>_<file_idx>.<ext> */
file_idx++;
chsnprintf(fname, 25, "%s_%05d.%s", path, file_idx, ext);
sderr = f_open(fp, fname, mode);
/* Existance Check */
if (sderr == FR_EXIST) {
continue;
} else {
if(sderr != FR_OK) {
/* Failed to Open File */
COMPONENT_STATE_UPDATE(avionics_component_sd_card, state_error);
}
return sderr;
}
}
}
void log_saver_thread(void *p)
{
chRegSetThreadName("logsaver");
char filename[16];
g_fileindex = next_free_filename();
chsnprintf(filename, sizeof(filename), "%04d.txt", g_fileindex);
unsigned bytes_written;
FIL file;
f_open(&file, filename, FA_WRITE | FA_CREATE_NEW);
static const char header[] = "# SysTime BattU BattI Tmotor Tmosfet RPM "
"AccX AccY AccZ GyrX TotAcc MotorTgt IAccum Brake MaxDuty\r\n"
"# ms mV mA mC mC "
" mg mg mg dps mg mA mA \r\n";
f_write(&file, header, sizeof(header) - 1, &bytes_written);
for (;;)
{
eventmask_t event = chEvtWaitOne(EVENT_BUF1 | EVENT_BUF2);
if (event & EVENT_BUF1)
{
f_write(&file, g_logbuffer1, sizeof(g_logbuffer1), &bytes_written);
}
if (event & EVENT_BUF2)
{
f_write(&file, g_logbuffer2, sizeof(g_logbuffer2), &bytes_written);
}
f_sync(&file);
}
}
static bool_t sendFile (uint8_t dest, uint8_t id, uint8_t page) {
char filename[20];
chsnprintf(filename, sizeof filename, "tosqa%03d.bin", id);
FRESULT res = f_stat(filename, &filinfo);
if (res == 0)
res = f_open(&fil, filename, FA_OPEN_EXISTING | FA_READ);
if (res != 0)
return false;
chprintf(chp1, "(%s: %db)", filename, filinfo.fsize);
chThdSleepMilliseconds(100);
CANTxFrame txmsg;
txmsg.IDE = CAN_IDE_EXT;
txmsg.EID = 0x1F123400 + dest;
txmsg.RTR = CAN_RTR_DATA;
txmsg.DLC = 8;
res = f_lseek(&fil, (page - 1) * 4096);
if (res == 0) {
int i;
for (i = 0; i < 512; ++i) {
UINT count;
res = f_read(&fil, txmsg.data8, 8, &count);
if (res != 0 || (i == 0 && count == 0))
break;
memset(txmsg.data8 + count, 0xFF, 8 - count);
canTransmit(&CAND1, 1, &txmsg, 100); // 1 mailbox, must send in-order!
// don't call echoToBridge for these bulk transfers
}
}
f_close(&fil);
return res == 0;
}
/* Render the cstring provided to an escaped version that can be printed. */
static char *print_string_ptr(const char *str)
{
const char *ptr;char *ptr2,*out;int len=0;unsigned char token;
if (!str) return cJSON_strdup("");
ptr=str;while ((token=*ptr) && ++len) {if (strchr("\"\\\b\f\n\r\t",token)) len++; else if (token<32) len+=5;ptr++;}
out=(char*)cJSON_malloc(len+3);
if (!out) return 0;
ptr2=out;ptr=str;
*ptr2++='\"';
while (*ptr)
{
if ((unsigned char)*ptr>31 && *ptr!='\"' && *ptr!='\\') *ptr2++=*ptr++;
else
{
*ptr2++='\\';
switch (token=*ptr++)
{
case '\\': *ptr2++='\\'; break;
case '\"': *ptr2++='\"'; break;
case '\b': *ptr2++='b'; break;
case '\f': *ptr2++='f'; break;
case '\n': *ptr2++='n'; break;
case '\r': *ptr2++='r'; break;
case '\t': *ptr2++='t'; break;
default: chsnprintf(ptr2,sizeof(ptr2),"u%04x",token);ptr2+=5; break; /* escape and print */
}
}
}
*ptr2++='\"';*ptr2++=0;
return out;
}
/* Render the number nicely from the given item into a string. */
static char *print_number(cJSON *item)
{
char *str;
double d=item->valuedouble;
if (fabs(((double)item->valueint)-d)<=DBL_EPSILON && d<=INT_MAX && d>=INT_MIN)
{
str=(char*)cJSON_malloc(21); /* 2^64+1 can be represented in 21 chars. */
chsnprintf(str,sizeof(str),"%d",item->valueint);
}
else
{
str=(char*)cJSON_malloc(64); /* This is a nice tradeoff. */
if (str)
{
if (fabs(floor(d)-d)<=DBL_EPSILON && fabs(d)<1.0e60)chsnprintf(str,sizeof(str),"%.0f",d);
else if (fabs(d)<1.0e-6 || fabs(d)>1.0e9) chsnprintf(str,sizeof(str),"%e",d);
else chsnprintf(str,sizeof(str),"%f",d);
}
}
return str;
}
static void cmd_zubax_id(BaseSequentialStream *chp, int argc, char *argv[])
{
if (argc == 0) {
// Product identification
printf("product_id : '%s'\n", NODE_NAME);
printf("product_name : 'PX4 Sapog'\n");
// SW version
printf("sw_version : '%u.%u'\n", FW_VERSION_MAJOR, FW_VERSION_MINOR);
printf("sw_vcs_commit: %u\n", unsigned(GIT_HASH));
printf("sw_build_date: %s\n", __DATE__);
// HW version
const auto hw_version = board::detect_hardware_version();
printf("hw_version : '%u.%u'\n", hw_version.major, hw_version.minor);
// Unique ID and signature
char base64_buf[os::base64::predictEncodedDataLength(std::tuple_size<board::DeviceSignature>::value) + 1];
std::array<std::uint8_t, 16> uid_128;
std::fill(std::begin(uid_128), std::end(uid_128), 0);
{
const auto uid = board::read_unique_id();
std::copy(std::begin(uid), std::end(uid), std::begin(uid_128));
}
printf("hw_unique_id : '%s'\n", os::base64::encode(uid_128, base64_buf));
board::DeviceSignature signature;
if (board::try_read_device_signature(signature)) {
printf("hw_signature : '%s'\n", os::base64::encode(signature, base64_buf));
std::memset(&base64_buf[0], 0, sizeof(base64_buf));
for (unsigned i = 0; i < 16; i++) {
chsnprintf(&base64_buf[i * 2], 3, "%02x", uid_128[i]);
}
printf("hw_info_url : http://device.zubax.com/device_info?uid=%s\n", &base64_buf[0]);
}
} else if (argc == 1) {
const char* const encoded = argv[0];
board::DeviceSignature sign;
if (!os::base64::decode(sign, encoded)) {
std::puts("Error: Invalid base64");
return;
}
if (!board::try_write_device_signature(sign)) {
std::puts("Error: Write failed");
return;
}
} else {
std::puts("Error: Invalid usage. Format: zubax_id [base64 signature]");
}
}
static THD_FUNCTION (ThreadPoller, arg) {
(void) arg;
char out[9];
chRegSetThreadName("poller");
while (TRUE) {
if (bmp085_status == 0) {
PollerData.temp = bmp085_read_temp();
PollerData.press = bmp085_read_press();
// PollerData.uTime = rtcGetTimeUnixSec(&RTCD1);
chsnprintf(out, sizeof(out), "%4.2f", PollerData.press/133.322);
lcd5110SetPosXY(30, 0);
lcd5110WriteText(out);
chsnprintf(out, sizeof(out), "%4.2f", PollerData.temp/10.0);
lcd5110SetPosXY(30, 1);
lcd5110WriteText(out);
}
chThdSleepMilliseconds(POLLER_TIMEOUT);
}
return 0; // never returns
}
// send incoming CAN message to current bridge session, if any
static void canToBridge (const CANRxFrame* rxMsg) {
if (currConn != 0) {
char buf [30];
uint32_t id = rxMsg->EID;
if (!rxMsg->IDE)
id &= 0x7FF;
chsnprintf(buf, sizeof buf, "S%x#", id);
char* p = buf + strlen(buf);
int i;
for (i = 0; i < rxMsg->DLC; ++i)
p = appendHex(p, rxMsg->data8[i]);
strcpy(p, "\r\n");
netconn_write(currConn, buf, strlen(buf), NETCONN_COPY);
}
}
/**
* Transmit APRS telemetry configuration
*/
uint32_t aprs_encode_telemetry_configuration(uint8_t* message, mod_t mod, telemetryConfig_t type)
{
char temp[4];
ax25_t packet;
packet.data = message;
packet.max_size = 512; // TODO: replace 512 with real size
packet.mod = mod;
ax25_send_header(&packet, APRS_CALLSIGN, APRS_SSID, APRS_PATH); // Header
ax25_send_byte(&packet, ':'); // Message flag
// Callsign
ax25_send_string(&packet, APRS_CALLSIGN);
ax25_send_byte(&packet, '-');
chsnprintf(temp, sizeof(temp), "%d", APRS_SSID);
ax25_send_string(&packet, temp);
ax25_send_string(&packet, " :"); // Message separator
switch(type) {
case CONFIG_PARM:
ax25_send_string(&packet, "PARM.Battery,Solar,Temp,Charge,Discharge");
break;
case CONFIG_UNIT:
ax25_send_string(&packet, "UNIT.Volt,Volt,degC,mW,mW");
break;
case CONFIG_EQNS:
ax25_send_string(&packet,
"EQNS."
"0,.001,0,"
"0,.001,0,"
"0,0.1,-100,"
"0,1,0,"
"0,1,0"
);
break;
case CONFIG_BITS:
ax25_send_string(&packet, "BITS.11111111,Pecan Balloon");
break;
}
ax25_send_footer(&packet); // Footer
nrzi_encode(&packet);
scramble(&packet);
return packet.size;
}
void log_writer_thread(void *p)
{
int write_next = EVENT_BUF1;
int writeptr = 0;
chRegSetThreadName("logwriter");
chThdSleepMilliseconds(2000);
for (;;)
{
chThdSleepMilliseconds(50);
int x, y, z, gx, gy, gz;
sensors_get_accel(&x, &y, &z);
sensors_get_gyro(&gx, &gy, &gz);
static char buf[512];
chsnprintf(buf, sizeof(buf),
"%8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d\r\n",
chVTGetSystemTime(),
get_battery_voltage_mV(), get_battery_current_mA(),
get_motor_temperature_mC(), get_mosfet_temperature_mC(),
motor_orientation_get_rpm(), x, y, z, gx,
bike_control_get_acceleration_level(), bike_control_get_motor_current(), bike_control_get_I_accumulator(),
!palReadPad(GPIOB, GPIOB_BRAKE), motor_limits_get_max_duty()
);
char *p = buf;
while (*p)
{
uint8_t *dest = (write_next == EVENT_BUF1) ? g_logbuffer1 : g_logbuffer2;
while (*p && writeptr < sizeof(g_logbuffer1))
{
dest[writeptr++] = *p++;
}
if (writeptr == sizeof(g_logbuffer1))
{
chEvtSignal(g_logsaver, write_next);
write_next = (write_next == EVENT_BUF1) ? EVENT_BUF2 : EVENT_BUF1;
writeptr = 0;
}
}
}
}
void motor_sampling_print(BaseSequentialStream *stream)
{
g_enable_sampling = false;
chprintf(stream, "#ORI HALL Ph1 Ph2 Ph3 I_Phase1 I_Phase3 I_real I_imag U_real U_imag\r\n");
for (int i = 0; i < MOTOR_SAMPLE_COUNT; i++)
{
motor_sample_t sample = g_motor_samples[(i + g_motor_samples_writeidx) % MOTOR_SAMPLE_COUNT];
char buf[128];
chsnprintf(buf, sizeof(buf), "%4d %4d %4d %4d %4d %8d %8d %4d %4d %4d %4d\r\n",
sample.motor_orientation * 2, sample.hall_angle * 2,
sample.ph1 * 4, sample.ph2 * 4, sample.ph3 * 4,
sample.current_ph1 * 100, sample.current_ph3 * 100,
sample.ivector_r, sample.ivector_i,
sample.uvector_r, sample.uvector_i);
chSequentialStreamWrite(stream, (void*)buf, strlen(buf));
}
g_enable_sampling = true;
}
/**
* Transmit APRS position packet. The comments are filled with:
* - Static comment (can be set in config.h)
* - Battery voltage in mV
* - Solar voltage in mW (if tracker is solar-enabled)
* - Temperature in Celcius
* - Air pressure in Pascal
* - Number of satellites being used
* - Number of cycles where GPS has been lost (if applicable in cycle)
*/
uint32_t aprs_encode_position(uint8_t* message, mod_t mod, trackPoint_t *trackPoint)
{
char temp[22];
ptime_t date = trackPoint->time;
ax25_t packet;
packet.data = message;
packet.max_size = 512; // TODO: replace 512 with real size
packet.mod = mod;
ax25_send_header(&packet, APRS_CALLSIGN, APRS_SSID, APRS_PATH);
ax25_send_byte(&packet, '/'); // Report w/ timestamp, no APRS messaging. $ = NMEA raw data
// 170915 = 17h:09m:15s zulu (not allowed in Status Reports)
chsnprintf(temp, sizeof(temp), "%02d%02d%02dh", date.hour, date.minute, date.second);
ax25_send_string(&packet, temp);
// Latitude
uint32_t y = 380926 * (90 - trackPoint->gps_lat/10000000.0);
uint32_t y3 = y / 753571;
uint32_t y3r = y % 753571;
uint32_t y2 = y3r / 8281;
uint32_t y2r = y3r % 8281;
uint32_t y1 = y2r / 91;
uint32_t y1r = y2r % 91;
// Longitude
uint32_t x = 190463 * (180 + trackPoint->gps_lon/10000000.0);
uint32_t x3 = x / 753571;
uint32_t x3r = x % 753571;
uint32_t x2 = x3r / 8281;
uint32_t x2r = x3r % 8281;
uint32_t x1 = x2r / 91;
uint32_t x1r = x2r % 91;
// Altitude
uint32_t a = logf(METER_TO_FEET(trackPoint->gps_alt)) / logf(1.002f);
uint32_t a1 = a / 91;
uint32_t a1r = a % 91;
uint8_t gpsFix = trackPoint->gps_lock ? GSP_FIX_CURRENT : GSP_FIX_OLD;
uint8_t src = NMEA_SRC_GGA;
uint8_t origin = ORIGIN_PICO;
temp[0] = SYM_GET_TABLE(APRS_SYMBOL);
temp[1] = y3+33;
temp[2] = y2+33;
temp[3] = y1+33;
temp[4] = y1r+33;
temp[5] = x3+33;
temp[6] = x2+33;
temp[7] = x1+33;
temp[8] = x1r+33;
temp[9] = SYM_GET_SYMBOL(APRS_SYMBOL);
temp[10] = a1+33;
temp[11] = a1r+33;
temp[12] = ((gpsFix << 5) | (src << 3) | origin) + 33;
temp[13] = 0;
ax25_send_string(&packet, temp);
// Comments
ax25_send_string(&packet, "SATS ");
chsnprintf(temp, sizeof(temp), "%d", trackPoint->gps_sats);
ax25_send_string(&packet, temp);
ax25_send_string(&packet, " TTFF ");
chsnprintf(temp, sizeof(temp), "%d", trackPoint->gps_ttff);
ax25_send_string(&packet, temp);
ax25_send_string(&packet, "sec");
// GPS Loss counter
if(!trackPoint->gps_lock)
{
ax25_send_string(&packet, " GPS LOSS ");
chsnprintf(temp, sizeof(temp), "%d", ++loss_of_gps_counter);
ax25_send_string(&packet, temp);
} else {
loss_of_gps_counter = 0;
}
temp[2] = 0;
ax25_send_byte(&packet, '|');
// Sequence ID
uint32_t t = trackPoint->id & 0x1FFF;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
// Battery voltage
t = trackPoint->adc_battery;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
// Solar voltage
t = trackPoint->adc_solar;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
// Temperature
t = trackPoint->air_temp/10 + 1000;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
// Battery charge
t = trackPoint->adc_charge;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
// Battery discharge
t = trackPoint->adc_discharge;
temp[0] = t/91 + 33;
temp[1] = t%91 + 33;
ax25_send_string(&packet, temp);
ax25_send_byte(&packet, '|');
ax25_send_footer(&packet);
nrzi_encode(&packet);
scramble(&packet);
return packet.size;
}
