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; }